﻿#include "algorithmunstackcommon.h"
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <QFile>
#include <algorithm>
#include <QJsonObject>
#include <QJsonDocument>
#include <QJsonArray>
#include <QPoint>
#include <QTextCodec>
#include <QCoreApplication>
#include "common_log.h"
#include "../../commonTools/commonfunctionset.h"
#include "../../visualTools/eigenfuncset.h"

using namespace std;
using namespace cv;

#define JIG_POSITION_FIX_X 0
#define JIG_POSITION_FIX_Y 0
#define UNSTACK_ANGLE_FIVE 10
#define UNSTACK_LAYER_GAP_2 50

bool compbyZ(BOXINFO& a1, BOXINFO& a2)
{
	return a1.Position.SkuPosZ > a2.Position.SkuPosZ;
}

//根据不同的配置选择不通过的排序算法
bool compbyXP(BOXINFO& a1, BOXINFO& a2)
{
	return a1.Position.SkuPosX > a2.Position.SkuPosX;
}

bool compbyYP(BOXINFO& a1, BOXINFO& a2)
{
	return a1.Position.SkuPosY > a2.Position.SkuPosY;
}

bool compbyintXP(int& x1, int& x2)
{
	return x1 > x2;
}

bool compbyintYP(int& y1, int& y2)
{
	return y1 > y2;
}

bool compbyXN(BOXINFO& a1, BOXINFO& a2)
{
	return a1.Position.SkuPosX < a2.Position.SkuPosX;
}

bool compbyYN(BOXINFO& a1, BOXINFO& a2)
{
	return a1.Position.SkuPosY < a2.Position.SkuPosY;
}

bool compbyintXN(int& x1, int& x2)
{
	return x1 < x2;
}

bool compbyintYN(int& y1, int& y2)
{
	return y1 < y2;
}

//根据不同的配置选择不同的排序算法，由大到小拆，由大到小拆
bool compbyYPXP(BOXINFO& a1, BOXINFO& a2)
{
	return a1.Position.SkuPosX + a1.Position.SkuPosY > a2.Position.SkuPosX + a2.Position.SkuPosY;
}
bool compbyXPYP(QPoint& p1, QPoint& p2)
{
	return p1.x() + p1.y() > p2.x() + p2.y();
}

//根据不同的配置选择不同的排序算法，X方向由大到小，Y方向由小到大
bool compbyYNXP(BOXINFO& a1, BOXINFO& a2)
{

	return a1.Position.SkuPosX - a1.Position.SkuPosY > a2.Position.SkuPosX - a2.Position.SkuPosY;
}
bool compbyXPYN(QPoint& p1, QPoint& p2)
{
	return p1.x() - p1.y() > p2.x() - p2.y();
}

//根据不同的配置选择不同的排序算法，X方向由小到大，Y方向由大到小
bool compbyYPXN(BOXINFO& a1, BOXINFO& a2)
{

	return -a1.Position.SkuPosX + a1.Position.SkuPosY > -a2.Position.SkuPosX + a2.Position.SkuPosY;
}
bool compbyXNYP(QPoint& p1, QPoint& p2)
{
	return -p1.x() + p1.y() > -p2.x() + p2.y();
}

//根据不同的配置选择不同的排序算法，X方向由小到大，Y方向由小到大
bool compbyYNXN(BOXINFO& a1, BOXINFO& a2)
{

	return -a1.Position.SkuPosX - a1.Position.SkuPosY > -a2.Position.SkuPosX - a2.Position.SkuPosY;
}
bool compbyXNYN(QPoint& p1, QPoint& p2)
{
	return -p1.x() - p1.y() > -p2.x() - p2.y();
}

QString imencodeToBase64(const cv::Mat& Image, const std::string& ext);
QVector<float> Eul2Quat(float yaw, float roll, float pitch);

AlgorithmUnstackCommon::AlgorithmUnstackCommon()
{
	beforinfo.btrue = false;
	bSave = false;
}

AlgorithmUnstackCommon::~AlgorithmUnstackCommon()
{
}

int AlgorithmUnstackCommon::Init(const visAlgParam& param)
{
	//初始化拆垛配置信息
	if (!SetUnstackSetting(param)) {
		log_error("拆垛配置信息错误");
		return -1;
	}
	QString calibration = QCoreApplication::applicationDirPath() + "/config/" + param.calibration;
	//初始化相机内参信息
	if (!SetIntrinsic(calibration)) {
		log_error("标定文件[{}]异常", param.calibration);
		return -2;
	}

	//初始化相机外参信息
	if (!SetExternal(calibration)) {
		log_error("标定文件[{}]异常", param.calibration);
		return -2;
	}

	//根据标定文件   设定托盘的范围
	if (!SetWorkSpace(calibration)) {
		log_error("标定文件[{}]异常", param.calibration);
		return -2;
	}

	return 0;
}

bool AlgorithmUnstackCommon::Caculate(const VisAlgInput& input, Position& pos, VisAlgOutput& out)
{
	QString info;
	cv::Mat resultImage;
	QString base64Image;
	//传入 0彩色图路径，1深度图路径，2识别结果，3抓取数量，4尺寸
	//在新的抓取逻辑中，只有当抓取数量且配置中开启双抓才能双抓
	//返回值
	//0成功返回抓取位置，包括托盘上无识别结果，即托盘上已经无货，1.获取识别结果失败
	//2获取箱子尺寸信息失败，3获取彩色图失败,4获取深度图数据失败
	//5托盘范围检验过滤掉了，6最高值过滤过滤掉了 7寻找相邻的箱子失败 8点云干涉校验不通过
	//9尺寸校验不通过
	log_trace("Caculate start: rgb:{}, depth:{}", input.rgb,
		input.depth);
	log_trace("识别数量：{}, taskNum:{}", input.boxs.size(), input.taskNum);
	log_trace("物料尺寸：长：{} 宽：{} 高：{}", input.sku.length, input.sku.width, input.sku.height);
	std::cout << "物料尺寸" << input.sku.length << "," << input.sku.width << "," << input.sku.height << std::endl;
	recog_result.clear();

	//从txt中获取识别结果
	QString ipt; // 二次识别求高度
	int size = ParseRecogRecogResult(input.boxs, recog_result);

	//非第一次时，输出差值（包括本次与上一次的高度差  上一次的识别结果（X,Y））
	if (beforinfo.btrue) {
		CalPointDifference(input.depth, beforinfo, ipt);
	}

	//获取本次抓取箱子尺寸，如果有的话
	boxLength = input.sku.length;
	boxWidth = input.sku.width;
	boxHeight = input.sku.height;

	//读取彩色图
	cv::Mat rgb = cv::imread(input.rgb.toStdString());
	if (rgb.empty()) {
		out.msg = "彩色图路径异常";
		return false;
	}

	//创建点云
	PointCloud::Ptr cloud_input_ptr(new PointCloud);//此为相机系
	cv::Mat depthori = cv::imread(input.depth.toStdString(), -1);
	int h = depthori.rows;
	int w = depthori.cols;
	cv::Mat gray;
	if (depthori.type() == CV_16UC1) {
		depthori.convertTo(depthori, CV_32FC1);
		gray = depthori;
	} else {
		cv::Mat depth_float(depthori.rows, depthori.cols, CV_32FC1, depthori.data);//转32位图像
		gray = depth_float;
	}

	if (gray.empty()) {
		out.msg = "深度图路径异常";
		return false;
	}

	for (int x = 0; x < h; x++) {
		for (int y = 0; y < w; y++) {
			PointT point_temp;
			float zz = gray.at<float>(x, y);
			point_temp.z = zz;
			point_temp.x = y;
			point_temp.y = x;

			if (point_temp.z <= 5000 && zz > 0) {
				cloud_input_ptr->points.push_back(point_temp);
			}

		}
	}

	if (bSave) {
		cloud_input_ptr->height = 1;
		cloud_input_ptr->width = cloud_input_ptr->points.size();
		pcl::io::savePCDFile("cloud_ini.pcd", *cloud_input_ptr);
	}

	//滤波d
	PointCloud::Ptr cloud_filter_ptr = Filter(cloud_input_ptr);

	if (bSave) {
		pcl::io::savePCDFile("cloud_filter.pcd", *cloud_filter_ptr);
	}

	//创建识别结果子点云，用于求识别结果空间位姿。
	//在此创建是为了避免识别结果角点深度值误差过大引起的其他误差
	QVector<PointCloud::Ptr> recog_cloud_ptr_vec = GetCaseCloudVec(cloud_filter_ptr);

	log_trace("cloud vec size = {}", recog_cloud_ptr_vec.size());

	//点云校验
	//建立夹具外形点云
	suker_size.setX(input.jig.length);
	suker_size.setY(input.jig.width);
	//获取吸盘类型
	suckerType = input.jig.type;
	PointCloud::Ptr sucker_convex(new PointCloud);
	sucker_convex = GetJigPoint();
	if (bSave) {
		pcl::io::savePCDFile("cloud_jig.pcd", *sucker_convex);
	}

	//坐标系转换  将切割之后的子点云转化到机械手坐标系中
	PointCloud::Ptr cloud_ptr_world(new PointCloud);
	cloud_ptr_world = pix2world(cloud_filter_ptr);

	for (int i = 0; i < cloud_ptr_world->size(); i++) {
		cloud_ptr_world->at(i).x = cloud_ptr_world->at(i).x / 1000.0;
		cloud_ptr_world->at(i).y = cloud_ptr_world->at(i).y / 1000.0;
		cloud_ptr_world->at(i).z = cloud_ptr_world->at(i).z / 1000.0;
	}

	//过滤，仅保留标定文件设定的空间范围内的点云
	PointCloud::Ptr cloud_input_fliter_ptr(new PointCloud);//此为相机系
	parrProcess(cloud_ptr_world, cloud_input_fliter_ptr);

	if (bSave) {
		cloud_input_fliter_ptr->height = 1;
		cloud_input_fliter_ptr->width = cloud_input_fliter_ptr->points.size();
		cloud_ptr_world->height = 1;
		cloud_ptr_world->width = cloud_ptr_world->points.size();
		pcl::io::savePCDFile("cloud_cover.pcd", *cloud_ptr_world);
		pcl::io::savePCDFile("cloud_input_fliter_ptr.pcd", *cloud_input_fliter_ptr);
	}

	//聚类
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloud;
	EuclideanClusterExtractionCloud(cloud_input_fliter_ptr, 0.02, 50, 9999999, &vCloud);
	//获取聚类后点云最高位置，即物料最高位置
	maxHeight = getMaxCloud(vCloud);

	//在RGB中画出托盘范围
	DrawTray(rgb);
	if (maxHeight < zlimit_min) {
		if (recog_result.size() <= 0) {
			out.msg = "托盘为空";
			out.num = 0;
			out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
			return true;
		} else {
			out.msg = "点云数据为空，但是视觉服务识别结果不为空";
			out.num = -1;
			out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
			return false;
		}
	}

	//识别结果
	QVector<RECGRESULT> recog_result2;
	QVector<PointCloud::Ptr> recog_cloud_ptr_vec2;
	QVector<cv::RotatedRect> vRect;
	for (int i = 0; i < recog_result.size(); i++) {
		PointCloud::Ptr tempPiex = recog_cloud_ptr_vec.at(i);

		//转到相机坐标系
		PointCloud::Ptr tempCam(new PointCloud);
		tempCam = pix2cam(tempPiex);

		if (tempCam->points.size() < 300) {
			continue;
		}

		//剪切特定方向点云 //根据向量方向进行裁剪
		pclClass.CropCloudNormalRange(tempPiex, 30, tempPiex, 'z');

		//点云聚类
		std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloud;
		pclClass.EuclideanClusterExtractionCloud(tempCam, 0.03, 50, 9999999, &vCloud);

		if (vCloud.size() == 0) {
			continue;
		}

		//获取最大点云
		pcl::PointCloud<pcl::PointXYZ>::Ptr Cloud3 = pclClass.getMaxCloud(vCloud);

		//平面分割
		pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudPlane;
		pcl::ModelCoefficients::Ptr pcoefficients;
		pclClass.PlaneSegmentation(Cloud3, 0.02, &pcoefficients, &pCloudPlane); //平面分割参数设置为1cm

		//计算平面角度
		float fAlpha, fBeta, fGamma, fDistance;
		pclClass.PlaneCoefficients2Euler(pcoefficients, &fAlpha, &fBeta, &fGamma, &fDistance);

		//todo  fGamma验证点云方向
		//-------------------------
		float anglex = 30;
		if (!(abs(fGamma) < anglex || abs(fGamma - 180) < anglex || abs(fGamma + 180) < anglex)) {
			continue;
		}
		//-------------------------

		if (bSave) {
			pCloudPlane->width = pCloudPlane->points.size();
			pCloudPlane->height = 1;
			pcl::io::savePCDFile("cloudTempCam" + std::to_string(i) + ".pcd", *pCloudPlane);
		}

		//转到像素坐标系
		PointCloud::Ptr cloudPix(new PointCloud);
		cloudPix = cam2pix(pCloudPlane);
		if (bSave) {
			cloudPix->width = cloudPix->points.size();
			cloudPix->height = 1;
			pcl::io::savePCDFile("cloudPix" + std::to_string(i) + ".pcd", *cloudPix);
		}

		//求解mask和最小包围盒
		cv::Mat matMask = pclClass.PCLCloud2cvCloudMask(pCloudPlane, gray.rows, gray.cols, intrinsicMatrix, distortionMatrix);
		cv::Mat matMaskPad;
		int pad = 5;
		UpsampleMask(matMask, matMaskPad, pad);
		cv::RotatedRect rect = getminAreaRect(matMaskPad);
		rect.size.width -= 2 * pad;
		rect.size.height -= 2 * pad;
		cv::Point2f box[4];
		rect.points(box);

		RECGRESULT tempRecg;
		tempRecg.id = recog_result[i].id;
		tempRecg.P1.setX(box[0].x);
		tempRecg.P1.setY(box[0].y);
		tempRecg.P2.setX(box[1].x);
		tempRecg.P2.setY(box[1].y);
		tempRecg.P3.setX(box[2].x);
		tempRecg.P3.setY(box[2].y);
		tempRecg.P4.setX(box[3].x);
		tempRecg.P4.setY(box[3].y);
		recog_result2.push_back(tempRecg);
		recog_cloud_ptr_vec2.push_back(pCloudPlane);
		vRect.push_back(rect);
		DrowBox(rgb, rect, tempRecg.id);
	}
	recog_result.clear();
	recog_cloud_ptr_vec.clear();
	recog_result = recog_result2;
	recog_cloud_ptr_vec = recog_cloud_ptr_vec2;

	//构造识别与计算结果
	//根据子点云  与  识别结果  得出  需要抓取的空间立方体的  长  宽  中心点  角度
	QVector<BOXINFO> recgvec;
	int id = 0;
	log_trace("GetCaseInfoFromPix:");
	for (int i = 0; i < recog_result.size(); i++) {
		BOXINFO tempcase = GetCaseInfoFromPix(recog_cloud_ptr_vec[i], recog_result[i]);
		//记录识别区域的ID   用于计算差值的时候输出
		recog_result[i].id = id;
		tempcase.id = id;
		id++;
		if (tempcase.SkuLength != 0) {
			recgvec.push_back(tempcase);
		}
		log_trace("number{},w:{},l:{} position {}, {}, {}, {}", i, tempcase.SkuWidth, tempcase.SkuLength,
			tempcase.Position.SkuPosX, tempcase.Position.SkuPosY, tempcase.Position.SkuPosZ, tempcase.Position.SkuPosZA);
	}

	//识别结果为空时
	if (recgvec.size() == 0) {
		out.msg = u8"托盘范围标定过滤删除";
		out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
		out.num = -1;
		return false;
	}

	//找出顶层结果
	std::sort(recgvec.begin(), recgvec.end(), compbyZ);
	if (!GetTopFloorCase(recgvec)) {
		out.msg = u8"顶层箱子数量为0";
		out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
		out.num = -1;
		return false;
	}

	//根据配置对识别出来的箱子进行排序
	if (relativeX == 1 && relativeY == 1)//由大到小，由大到小
		std::sort(recgvec.begin(), recgvec.end(), compbyYPXP);
	else if (relativeX == 1 && relativeY == 0)
		std::sort(recgvec.begin(), recgvec.end(), compbyYNXP);
	else if (relativeX == 0 && relativeY == 1)
		std::sort(recgvec.begin(), recgvec.end(), compbyYPXN);
	else if (relativeX == 0 && relativeY == 0)
		std::sort(recgvec.begin(), recgvec.end(), compbyYNXN);

	//获取水平或者竖直方向相邻箱体,获取的箱子哪个在前，哪个在后
	QVector<QVector<BOXINFO>> neatcase = FindNeatCase(recgvec);
	if (neatcase.size() == 0) {
		out.msg = u8"获取相邻的箱子失败";
		out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
		out.num = -1;
		return false;
	}

	//根据最多可以拆几个箱子。将相邻的箱子多于该数的拆分
	int maxGrabNum = std::min(std::min(int(floor(800 / neatcase[0][0].SkuWidth)), GrabNum), input.taskNum);
	if (maxGrabNum <= 0) {
		out.msg = u8"抓取数量为 0";
		out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
		out.num = -1;
		return false;
	}
	BlockCheck(neatcase, maxGrabNum);

	//重新排序，将包含必须拆的箱子放在开头
	int firstId = recgvec.at(0).id;
	int i, j;
	for (i = 0; i < neatcase.size(); i++) {
		bool b = false;
		for (j = 0; j < neatcase.at(i).size(); j++) {
			if (neatcase.at(i).at(j).id == firstId) {
				b = true;
				break;
			}
		}
		if (b)
			break;
	}

	//将i所在的放在最前边
	QVector<BOXINFO> temp = neatcase.at(i);
	neatcase.erase(neatcase.begin() + i);
	neatcase.insert(neatcase.begin(), temp);

	//找到路径干涉最小的箱子，以及其在neatcase里的位置
	int indexX = 0;
	int indexY = 0;
	int  p = 10000;
	for (int i = 0; i < neatcase.size(); i++) {
		for (int j = 0; j < neatcase.at(i).size(); j++) {
			//求解路径干涉
			BOXINFO caseinfo = neatcase.at(i).at(j);
			//扣掉箱子处的点云
			PointCloud::Ptr cloud_check_ptr = GetCloudWithoutCase(caseinfo, cloud_ptr_world);
			int num = BoxCheck(caseinfo, cloud_check_ptr);
		}
		if (p < 400) {
			break;
		}
	}

	//选出必须拆的一组箱子
	QVector<BOXINFO> mustUnstack = neatcase.at(indexX);
	for (int i = 0; i < mustUnstack.size(); i++) {
		log_trace("i = {},indexX{} box({}, {})",
			mustUnstack.at(i).id, indexX, mustUnstack.at(i).Position.SkuPosX, mustUnstack.at(i).Position.SkuPosY);
	}

	//尺寸校验
	//当传入的尺寸信息不为零且开启尺寸校验时
	int error = 50;
	for (int i = 0; i < mustUnstack.size(); i++) {
		BOXINFO tempcase = mustUnstack.at(i);
		if (boxLength > 0 && boxWidth > 0 && boxHeight > 0 && SizeCheck) {
			if (abs(tempcase.SkuLength - boxLength) > error || abs(tempcase.SkuWidth - boxWidth) > error ||
				tempcase.SkuLength > maxBoxLength || tempcase.SkuLength < minBoxLength ||
				tempcase.SkuWidth > maxBoxWidth || tempcase.SkuWidth < minBoxWidth) {
				log_trace("sku size l{}, w{}",
					tempcase.SkuLength, tempcase.SkuWidth);
				std::cout << "识别的物料尺寸" << tempcase.SkuLength << "," << tempcase.SkuWidth << std::endl;

				out.msg = u8"尺寸校验不通过";
				out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
				out.num = -1;
				return false;
			}
		}
		if (tempcase.SkuLength > maxBoxLength || tempcase.SkuLength < minBoxLength ||
			tempcase.SkuWidth > maxBoxWidth || tempcase.SkuWidth < minBoxWidth) {
			out.msg = u8"识别尺寸不在范围内";
			out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
			out.num = -1;
			return false;
		}
	}

	
	QVector<QVector<BOXINFO>> neatcaseTmp = { mustUnstack };
	//多抓算法
	if (maxGrabNum >= 2) {
		GrabInfo grab;
		BOXINFO caseinfo;
		int num = trySeveralGrab(neatcaseTmp, cloud_ptr_world, maxGrabNum, input.jig, grab, caseinfo);
		if (num > 0) {
			Position skupos = { caseinfo.Position.SkuPosX, caseinfo.Position.SkuPosY, caseinfo.Position.SkuPosZ,
			caseinfo.Position.SkuPosZA, caseinfo.Position.SkuPosYA, caseinfo.Position.SkuPosXA };
			out.msg = u8"识别成功";
			out.num = num;
			out.jigInSku = grab.jigInsku;
			log_trace("jigInSku {},{},{},{}", out.jigInSku.x, out.jigInSku.y, out.jigInSku.z, out.jigInSku.a);
			Position jigInworld = CommonFun::TransAToBCoordinateSystem(grab.jigInsku, skupos);
			log_trace("jig position {},{},{},{},{},{}", jigInworld.x, jigInworld.y, jigInworld.z, jigInworld.a, jigInworld.b, jigInworld.c);
			pos = CommonFun::TransAToBCoordinateSystem(input.jig.rob, jigInworld);
			log_trace("grab position {},{},{},{},{},{}", pos.x, pos.y, pos.z, pos.a, pos.b, pos.c);

			UpdateBeforeinfo(caseinfo, 0, 0, beforinfo);
			DrowCoordinate(caseinfo.Position, rgb, 0, 0, 1);
			DrawSucker(jigInworld, input.jig, rgb);
			out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
			log_trace("position {},{},{},{}", pos.x, pos.y, pos.z, pos.a);
			return true;
		}
	}

	//单拆
	QVector<BOXINFO> vBox = neatcaseTmp.at(0);
	BOXINFO caseinfo = vBox.at(indexY);

	//扣掉箱子处的点云
	PointCloud::Ptr cloud_check_ptr = GetCloudWithoutCase(caseinfo, cloud_ptr_world);

	cloud_check_ptr->height = 1;
	cloud_check_ptr->width = cloud_check_ptr->points.size();
	if (bSave) {
		pcl::io::savePCDFile("cloud_singlewithoutcase.pcd", *cloud_check_ptr);
	}

	//偏移量绝对值
	float delta_x = 0, delta_y = 0;

	// 获取合适的抓取位姿
	GrabInfo grab = GetGrabPosition(caseinfo, input.jig, cloud_check_ptr);
	
	if (grab.id > 0) {
		Position skupos = { caseinfo.Position.SkuPosX, caseinfo.Position.SkuPosY, caseinfo.Position.SkuPosZ,
					caseinfo.Position.SkuPosZA, caseinfo.Position.SkuPosYA, caseinfo.Position.SkuPosXA };
		out.msg = u8"识别成功";
		out.num = 1;
		out.jigInSku = grab.jigInsku;
		log_trace("jigInSku {},{},{},{}", out.jigInSku.x, out.jigInSku.y, out.jigInSku.z, out.jigInSku.a);
		Position jigInworld = CommonFun::TransAToBCoordinateSystem(grab.jigInsku, skupos);
		log_trace("jig position {},{},{},{},{},{}", jigInworld.x, jigInworld.y, jigInworld.z, jigInworld.a, jigInworld.b, jigInworld.c);
		pos = CommonFun::TransAToBCoordinateSystem(input.jig.rob, jigInworld);
		log_trace("grab position {},{},{},{},{},{}", pos.x, pos.y, pos.z, pos.a, pos.b, pos.c);

		UpdateBeforeinfo(caseinfo, delta_x, delta_y, beforinfo);
		DrowCoordinate(caseinfo.Position, rgb, 0, 0, 1);
		DrawSucker(jigInworld, input.jig, rgb);
		//DrowSucker(pos.a * M_PI / 180, caseinfo, 0, 0, rgb);
		out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
		log_trace("position {},{},{},{}", pos.x, pos.y, pos.z, pos.a);
		return true;
	}
	out.msg = u8"点云干涉校验不通过";
	out.rgb = EigenFunc::ImgEncodeToByte(rgb, ".png");
	return false;
}

QVector<GrabInfo> AlgorithmUnstackCommon::GetGrabPosVec(BOXINFO& recog, const JigSetting& jig)
{
	auto w = recog.SkuWidth / 2;
	auto l = recog.SkuLength / 2;
	auto& jw = jig.width;
	auto& jl = jig.length;

	int area = recog.SkuWidth * recog.SkuLength; // 物料的表面积
	int s1 = std::min(2 * l, jl) * std::min(2 * w, jw);	// 夹具和物料的接触面积(平行抓取)
	int s2 = std::min(2 * l, jw) * std::min(2 * w, jl);	// 夹具和物料的接触面积(垂直抓取)
	QVector<GrabInfo> grabInfo;
	//grabInfo.resize(9);
	// 共有9中抓取位姿
	Position pos1 = { l, w, 0, 0, 0, 0 };
	GrabInfo p1 = { 1 };
	p1.jigInsku = CommonFun::FromAToBEuler(pos1);
	Position pos2 = { w, l, 0, 90, 0, 0 };
	GrabInfo p2 = { 2 };
	p2.jigInsku = CommonFun::FromAToBEuler(pos2);
	Position pos3 = { l, jw - w, 0, 0, 0, 0 };
	GrabInfo p3 = { 3 };
	p3.jigInsku = CommonFun::FromAToBEuler(pos3);
	Position pos4 = { w, jw - l, 0, 90, 0, 0 };
	GrabInfo p4 = { 4 };
	p4.jigInsku = CommonFun::FromAToBEuler(pos4);
	Position pos5 = { jl - l, w, 0, 0, 0, 0 };
	GrabInfo p5 = { 5 };
	p5.jigInsku = CommonFun::FromAToBEuler(pos5);
	Position pos6 = { jl - w, l, 0, 90, 0, 0 };
	GrabInfo p6 = { 6 };
	p6.jigInsku = CommonFun::FromAToBEuler(pos6);
	Position pos7 = { jl - l, jw - w, 0, 0, 0, 0 };
	GrabInfo p7 = { 7 };
	p7.jigInsku = CommonFun::FromAToBEuler(pos7);
	Position pos8 = { jl - w, jw - l, 0, 90, 0, 0 };
	GrabInfo p8 = { 8 };
	p8.jigInsku = CommonFun::FromAToBEuler(pos8);
	if (3 * s1 >= 2 * area) {	// 接触面积小于物料面积的2/3，则该抓取方式无效
		int socer = s1 > s2 ? 20 : 15;
		p1.socer = p3.socer = p5.socer = p7.socer = socer;
		grabInfo << p1 << p3 << p5 << p7;
	}

	//if (3 * s2 >= 2 * area) {	// 接触面积小于物料面积的2/3，则该抓取方式无效
	//	int socer = s2 > s1 ? 20 : 15;
	//	p2.socer = p4.socer = p6.socer = p8.socer = socer;
	//	grabInfo << p2 << p4 << p6 << p8;
	//}
	SortGrabPos(grabInfo);
	
	for (int i = 0; i < grabInfo.size(); i++) {
		grabInfo[i].socer -= grabInfo[i].id;
	}

	return grabInfo;
}

// 夹具坐标系：以一个角为原点，长为x,宽为y,满足右手直角坐标系
#define INTERFERE_NUM (20)
GrabInfo AlgorithmUnstackCommon::GetGrabPosition(BOXINFO& recog, const JigSetting& jig, PointCloud::Ptr world)
{
	QVector<GrabInfo> allPos = GetGrabPosVec(recog, jig);
	auto jigCloud = GetJigPoint(jig);
	Position skupos = { recog.Position.SkuPosX, recog.Position.SkuPosY, recog.Position.SkuPosZ,
						recog.Position.SkuPosZA, recog.Position.SkuPosYA, recog.Position.SkuPosXA };
	int i = 0;
	QVector<GrabInfo> grab;
	for (auto& iter : allPos) {
		Position jigInworld = CommonFun::TransAToBCoordinateSystem(iter.jigInsku, skupos);
		//变换矩阵
		Eigen::Affine3f transform = Eigen::Affine3f::Identity();
		transform.translation() << (jigInworld.x), (jigInworld.y), jigInworld.z;
		transform.rotate(Eigen::AngleAxisf(jigInworld.a * M_PI / 180, Eigen::Vector3f::UnitZ()));


		PointCloud::Ptr cloud_world(new PointCloud);
		pcl::transformPointCloud(*jigCloud, *cloud_world, transform);

		for (int j = 0; j < cloud_world->points.size(); j++) {
			cloud_world->points.at(j).x /= 1000.0;
			cloud_world->points.at(j).y /= 1000.0;
			cloud_world->points.at(j).z /= 1000.0;
		}

		cloud_world->height = 1;
		cloud_world->width = cloud_world->points.size();

		if (bSave) {
			pcl::io::savePCDFile("cloud_sucker" + QString::number(i).toStdString() + ".pcd", *cloud_world);
		}

		int numPoints = getPointIntersection(cloud_world, world);
		log_trace("num points{}, i = {}, socer = {}", numPoints, iter.id, iter.socer);
		if (numPoints > INTERFERE_NUM) {	// 20 : 点云干涉数量
			continue;
		}
		if (m_dir == 0) {
			iter.socer += (jigInworld.x + jigInworld.y);
		} else if (m_dir == 1) {
			iter.socer += (-jigInworld.x + jigInworld.y);
		} else if (m_dir == 2) {
			iter.socer += (-jigInworld.x - jigInworld.y);
		} else if (m_dir == 3) {
			iter.socer += (jigInworld.x - jigInworld.y);
		} else {
			log_error("抓取方向配置异常");
		}

		iter.socer += (INTERFERE_NUM - numPoints);
		iter.jigCloud = cloud_world;
		grab.push_back(iter);
	}
	if (grab.size() <= 0) {
		log_error("not find grab pos");
		return {};
	}

	std::sort(grab.begin(), grab.end(), [](GrabInfo& a, GrabInfo& b) {
		return a.socer > b.socer;
		});
	log_trace("jiginsku id:{} pos :({} {} {} {} {} {})", grab.at(0).id, grab.at(0).jigInsku.x, grab.at(0).jigInsku.y,
		grab.at(0).jigInsku.z, grab.at(0).jigInsku.a, grab.at(0).jigInsku.b, grab.at(0).jigInsku.c);
	return grab.at(0);
}

PointCloud::Ptr AlgorithmUnstackCommon::pix2world(PointCloud::Ptr cloud)
{
	//只有一个标定矩阵时
	if (intrinsic_matrix[0][0] == 0) {
		PointCloud::Ptr res(new PointCloud);
		Eigen::Matrix4f external;
		res->width = cloud->width;
		res->height = cloud->height;
		res->is_dense = cloud->is_dense;
		PointCloud::Ptr cloud_copy(new PointCloud);
		cloud_copy->points.resize(cloud->points.size());
		for (int i = 0; i < cloud->size(); i++) {
			cloud_copy->at(i).x = cloud->at(i).x * cloud->at(i).z;
			cloud_copy->at(i).y = cloud->at(i).y * cloud->at(i).z;
			cloud_copy->at(i).z = cloud->at(i).z;
		}

		for (int i = 0; i < 4; i++) //相机系-》世界系
		{
			for (int j = 0; j < 4; j++) {
				external(i, j) = external_matrix[i][j];
			}
		}
		pcl::transformPointCloud(*cloud_copy, *res, external);

		return res;
	}

	//内参与外参分开表示时
	else {
		PointCloud::Ptr res(new PointCloud);
		Eigen::Matrix4f external;
		res->width = cloud->width;
		res->height = cloud->height;
		res->is_dense = cloud->is_dense;

		PointCloud::Ptr cloud_copy(new PointCloud);
		cloud_copy->points.resize(cloud->points.size());
		for (int i = 0; i < cloud->points.size(); i++) //图像系-》相机系
		{
			cloud_copy->points.at(i).x = (cloud->points.at(i).x - intrinsic_matrix[0][2]) * cloud->points.at(i).z / intrinsic_matrix[0][0];//已经乘过Z了
			cloud_copy->points.at(i).y = (cloud->points.at(i).y - intrinsic_matrix[1][2]) * cloud->points.at(i).z / intrinsic_matrix[1][1];
			cloud_copy->at(i).z = cloud->at(i).z;
		}

		for (int i = 0; i < 4; i++)//相机系-》世界系
		{
			for (int j = 0; j < 4; j++) {
				external(i, j) = external_matrix[i][j];
			}
		}
		pcl::transformPointCloud(*cloud_copy, *res, external);

		return res;
	}

}

#define EXPAND (20)		// 夹具厚度
#define EXTEND 30
PointCloud::Ptr AlgorithmUnstackCommon::GetJigPoint(const JigSetting& info)
{
	PointCloud::Ptr jig(new PointCloud);

	jig->push_back(PointT(0, 0, -20));
	jig->push_back(PointT(0, 0, EXPAND));
	jig->push_back(PointT(0, info.width, -20));
	jig->push_back(PointT(0, info.width, EXPAND));
	jig->push_back(PointT(info.length, info.width, -20));
	jig->push_back(PointT(info.length, info.width, EXPAND));
	jig->push_back(PointT(info.length, 0, -20));
	jig->push_back(PointT(info.length, 0, EXPAND));

	if (info.type == 2) {	// 带侧吸
		jig->push_back(PointT(0, 0, -EXPAND));
		jig->push_back(PointT(info.length, 0, -EXPAND));
		jig->push_back(PointT(0, -20, EXPAND));
		jig->push_back(PointT(0, -20, -EXPAND));
		jig->push_back(PointT(info.length, -20, EXPAND));
		jig->push_back(PointT(info.length, -20, -EXPAND));
	}
	//pcl::io::savePCDFile("cloud_jig_original.pcd", *jig);
	return jig;
}

PointCloud::Ptr AlgorithmUnstackCommon::GetJigPoint()
{
	PointCloud::Ptr sucker_convex(new PointCloud);

	if (suckerType == 1) {
		sucker_convex->push_back(PointT(suker_size.x() / 2, suker_size.y() / 2, -EXPAND));
		sucker_convex->push_back(PointT(-suker_size.x() / 2, suker_size.y() / 2, -EXPAND));
		sucker_convex->push_back(PointT(suker_size.x() / 2, -suker_size.y() / 2, -EXPAND));
		sucker_convex->push_back(PointT(-suker_size.x() / 2, -suker_size.y() / 2, -EXPAND));
		sucker_convex->push_back(PointT(suker_size.x() / 2, suker_size.y() / 2, EXPAND * 5));
		sucker_convex->push_back(PointT(-suker_size.x() / 2, suker_size.y() / 2, EXPAND * 5));
		sucker_convex->push_back(PointT(suker_size.x() / 2, -suker_size.y() / 2, EXPAND * 5));
		sucker_convex->push_back(PointT(-suker_size.x() / 2, -suker_size.y() / 2, EXPAND * 5));

		if (bSave) {
			pcl::io::savePCDFile("cloud_jig_original.pcd", *sucker_convex);
		}

		return sucker_convex;
	} else {
		sucker_convex->push_back(PointT(suker_size.x() / 2, suker_size.y() / 2, -EXPAND * 2));
		sucker_convex->push_back(PointT(-suker_size.x() / 2, suker_size.y() / 2, -EXPAND * 2));
		sucker_convex->push_back(PointT(suker_size.x() / 2, -suker_size.y() / 2 - 250, -EXPAND * 2));
		sucker_convex->push_back(PointT(-suker_size.x() / 2, -suker_size.y() / 2 - 250, -EXPAND * 2));
		sucker_convex->push_back(PointT(suker_size.x() / 2, suker_size.y() / 2, EXPAND * 5));
		sucker_convex->push_back(PointT(-suker_size.x() / 2, suker_size.y() / 2, EXPAND * 5));
		sucker_convex->push_back(PointT(suker_size.x() / 2, -suker_size.y() / 2 - 250, EXPAND * 5));
		sucker_convex->push_back(PointT(-suker_size.x() / 2, -suker_size.y() / 2 - 250, EXPAND * 5));

		if (bSave) {
			pcl::io::savePCDFile("cloud_jig_original.pcd", *sucker_convex);
		}
		return sucker_convex;
	}
}

int AlgorithmUnstackCommon::PointCloudInterferenceCheck(BOXINFO recog, PointCloud::Ptr sucker_convex, PointCloud::Ptr cloud_filter_ptr, float& delx, float& dely, cv::Mat& image)
{
	//recog 可抓多个箱子的最小外接位姿
	//sucker_convex构建的夹具点云
	//cloud_filter_ptr 扣完箱子之后的场景电点云
	//delx 抓箱子时的X偏移
	//dely 抓箱子时的y偏移
	float radian = recog.Position.SkuPosZA * M_PI / 180.0;
	float jiglength = suker_size.x();
	float jigwidth = suker_size.y();
	float delta_tempx = jiglength >= recog.SkuLength ? (jiglength - recog.SkuLength) / 2 : (recog.SkuLength - jiglength) / 2;
	float delta_tempy = jigwidth >= recog.SkuWidth ? (jigwidth - recog.SkuWidth) / 2 : (recog.SkuWidth - jigwidth) / 2;
	std::vector<QPoint> jigs;
	jigs.push_back(QPoint(delta_tempx * qCos(radian) - delta_tempy * qSin(radian), delta_tempy * qCos(radian) + delta_tempx * qSin(radian)));
	jigs.push_back(QPoint(delta_tempx * qCos(radian) + delta_tempy * qSin(radian), -delta_tempy * qCos(radian) + delta_tempx * qSin(radian)));
	jigs.push_back(QPoint(-delta_tempx * qCos(radian) - delta_tempy * qSin(radian), delta_tempy * qCos(radian) - delta_tempx * qSin(radian)));
	jigs.push_back(QPoint(-delta_tempx * qCos(radian) + delta_tempy * qSin(radian), -delta_tempy * qCos(radian) - delta_tempx * qSin(radian)));

	if (relativeX == 1 && relativeY == 1)
		std::sort(jigs.begin(), jigs.end(), compbyXPYP);
	else if (relativeX == 1 && relativeY == 0)
		std::sort(jigs.begin(), jigs.end(), compbyXPYN);
	else if (relativeX == 0 && relativeY == 1)
		std::sort(jigs.begin(), jigs.end(), compbyXNYP);
	else if (relativeX == 0 && relativeY == 0)
		std::sort(jigs.begin(), jigs.end(), compbyXNYN);
	jigs.insert(jigs.begin(), QPoint(0, 0));

	for (int i = 1; i < 5; i++) {
		//5种方式都尝试验证 看看有无干涉（中间对齐＋4个角点对齐），计算角点时优先靠近机械手位置
		delx = jigs[i].x();
		dely = jigs[i].y();

		//float deltxx = delx == 0 ? 0 : (delx / abs(delx)) * (abs(delx) + 60);
		//float deltyy = dely == 0 ? 0 : (dely / abs(dely)) * (abs(dely) + 60);

		float deltxx = delx;
		float deltyy = dely;

		float zradian = recog.Position.SkuPosZA * M_PI / 180;

		if (suckerType == 2) {
			//临沂烟草是X从大到小，Y从小到大
			//如果是侧边吸盘需要进行角度映射
			//根据配置
			if (relativeX == 0 && relativeY == 0) {
				//XY都是从小到大拆
				zradian = GetAngleTrans(zradian * 180 / M_PI, 0, -90) * M_PI / 180;
			}
			if (relativeX == 0 && relativeY == 1) {
				//X从小到大，y从大到小
				zradian = GetAngleTrans(zradian * 180 / M_PI, 180, -90) * M_PI / 180;
			}
			if (relativeX == 1 && relativeY == 0) {
				//x从大到小，y从小到大
				zradian = GetAngleTrans(zradian * 180 / M_PI, 0, 90) * M_PI / 180;
			}
			if (relativeX == 1 && relativeY == 1) {
				//x从大到小，y从大到小
				zradian = GetAngleTrans(zradian * 180 / M_PI, 180, 90) * M_PI / 180;
			}
		}

		//夹具点云坐标变换
		Eigen::Affine3f transform = Eigen::Affine3f::Identity();
		transform.translation() << recog.Position.SkuPosX + deltxx, recog.Position.SkuPosY + deltyy, recog.Position.SkuPosZ;
		transform.rotate(Eigen::AngleAxisf(zradian, Eigen::Vector3f::UnitZ()));
		PointCloud::Ptr cloud_suckerconvex_world(new PointCloud);
		pcl::transformPointCloud(*sucker_convex, *cloud_suckerconvex_world, transform);

		for (int j = 0; j < cloud_suckerconvex_world->points.size(); j++) {
			cloud_suckerconvex_world->points.at(j).x /= 1000.0;
			cloud_suckerconvex_world->points.at(j).y /= 1000.0;
			cloud_suckerconvex_world->points.at(j).z /= 1000.0;
		}

		cloud_suckerconvex_world->height = 1;
		cloud_suckerconvex_world->width = cloud_suckerconvex_world->points.size();

		if (bSave) {
			pcl::io::savePCDFile("cloud_sucker" + QString::number(i).toStdString() + ".pcd", *cloud_suckerconvex_world);
		}

		float xmin;
		float xmax;
		float ymin;
		float ymax;
		float zmin;

		int numPoints = getPointIntersection(cloud_suckerconvex_world, cloud_filter_ptr);
		log_trace("num points{},x_idff{}, y_diff{}", numPoints, deltxx, deltyy);
		if (numPoints <= 200) {//验证两个点云是否有交集
			if (i != 0) {
				if (delx > JIG_POSITION_FIX_X) {
					delx -= JIG_POSITION_FIX_X;
				} else if (delx < -JIG_POSITION_FIX_X) {
					delx += JIG_POSITION_FIX_X;
				}

				if (dely > JIG_POSITION_FIX_Y) {
					dely -= JIG_POSITION_FIX_Y;
				} else if (dely < -JIG_POSITION_FIX_Y) {
					dely += JIG_POSITION_FIX_Y;
				}
			}
			DrowSucker(zradian, recog, delx, dely, image);
			return 1;
		}
	}

	return 0;

}

int AlgorithmUnstackCommon::getPointIntersection(PointCloud::Ptr point_hull, PointCloud::Ptr point_source)
{
	pcl::ConvexHull<PointT> sucker_hull;
	sucker_hull.setInputCloud(point_hull);
	sucker_hull.setDimension(3);
	std::vector<pcl::Vertices> sucker_polygons;
	PointCloud::Ptr sucker_surface_hull(new PointCloud);
	sucker_hull.reconstruct(*sucker_surface_hull, sucker_polygons);

	PointCloud::Ptr temp(new PointCloud);
	pcl::CropHull<PointT> sucker_ch_filter;
	sucker_ch_filter.setInputCloud(point_source);
	sucker_ch_filter.setDim(3);
	sucker_ch_filter.setHullIndices(sucker_polygons);
	sucker_ch_filter.setHullCloud(sucker_surface_hull);
	sucker_ch_filter.filter(*temp);

	return temp->size();
}

BOXINFO AlgorithmUnstackCommon::GetCaseInfoFromPix(PointCloud::Ptr cloud_case, RECGRESULT recg)
{
	//初始化各个参数
	BOXINFO caseinfo;
	caseinfo.SkuLength = 0;
	caseinfo.SkuWidth = 0;
	caseinfo.Position = { "",0 };

	//单位换算
	for (int i = 0; i < cloud_case->points.size(); i++) {
		cloud_case->points[i].z = cloud_case->points[i].z * 1000;
	}

	// 参数化分割
	//setOptimizeCoefficients 模型参数是否需要优化
	//setModelType            提取目标模型的属性（平面、球、圆柱等等）
	//setMethodType           采样方法（RANSAC、LMedS等）
	//setMaxIterations        最大迭代次数
	//setDistanceThreshold    查询点到目标模型的距离阈值（如果大于此阈值，则查询点不在目标模型上，默认值为0）。
	//segment                 输出提取点的索引和目标模型的参数。
	//ModelCoefficients       目标模型的参数。（平面、球、圆柱等等）
	//PointIndices            提取点的索引。
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	// Create the segmentation object
	pcl::SACSegmentation<PointT> seg;
	PointCloud::Ptr cloud_plane(new PointCloud);
	// Optional
	seg.setOptimizeCoefficients(true);
	// Mandatory
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(500);
	seg.setDistanceThreshold(5);//单位mm
	seg.setInputCloud(cloud_case);
	seg.segment(*inliers, *coefficients);
	// 提取索引
	pcl::ExtractIndices<PointT> extract;
	// Extract the inliers
	extract.setInputCloud(cloud_case);
	extract.setIndices(inliers);       // 设置分割后的内点为需要提取的点集
	extract.setNegative(false);        // 设置提取内点
	extract.filter(*cloud_plane);          // 提取并保存至 cloud_p

	//计算质心
	Eigen::Vector4f centroid;  //质心
	pcl::compute3DCentroid(*cloud_plane, centroid);

	//计算平均高度
	double h = 0;
	for (int i = 0; i < cloud_plane->points.size(); i++) {
		h += cloud_plane->points[i].z;
	}
	h = h / cloud_plane->points.size();

	//构建识别区域点云   添加高度
	//转化到机械手坐标系下
	PointCloud::Ptr cloud_caseconer_pix(new PointCloud);
	cloud_caseconer_pix->push_back(PointT(recg.P1.x(), recg.P1.y(), h));
	cloud_caseconer_pix->push_back(PointT(recg.P2.x(), recg.P2.y(), h));
	cloud_caseconer_pix->push_back(PointT(recg.P3.x(), recg.P3.y(), h));
	cloud_caseconer_pix->push_back(PointT(recg.P4.x(), recg.P4.y(), h));//centroid[2]
	PointCloud::Ptr cloud_caseconer_world = pix2world(cloud_caseconer_pix);

	cloud_caseconer_world->height = 1;
	cloud_caseconer_world->width = cloud_caseconer_world->points.size();

	if (bSave) {
		pcl::io::savePCDFile("cloud_childcover.pcd", *cloud_caseconer_world);
	}

	//平面点云最小包围盒及位姿
	pcl::PointXYZ OBBPosition;
	Eigen::Matrix3f OBBRotationalMatrix_;
	Eigen::Vector3f OBBSize;
	pcl::PointXYZ  MassCenter;
	pclClass.PlaneOBB(cloud_caseconer_world, &OBBPosition, &OBBRotationalMatrix_, &OBBSize, &MassCenter);

	PointCloud::Ptr cloud_space1(new PointCloud);
	cloud_space1 = cloud_caseconer_world;

	//构建托盘点云  
	//判断识别区域转化的点云是否存在与托盘点云内
	PointCloud::Ptr cloud_space(new PointCloud);
	pcl::CropBox<PointT> box_filter;
	box_filter.setMin(Eigen::Vector4f(xlimit_min, ylimit_min, zlimit_min, 1.));
	box_filter.setMax(Eigen::Vector4f(xlimit_max, ylimit_max, zlimit_max, 1.));
	box_filter.setInputCloud(cloud_caseconer_world);
	box_filter.setNegative(false);
	box_filter.filter(*cloud_space);

	PointCloud::Ptr cloud_space2(new PointCloud);

	if (bSave) {
		pcl::io::savePCDFile("cloud_stack.pcd", *cloud_caseconer_world);

	}

	cloud_space2 = cloud_space;

	if (cloud_space->points.size() < 4) {
		return caseinfo;
	}
	//判断识别区域点云值是否为nan
	for (size_t i = 0; i < cloud_space->points.size(); i++) {
		if (isnan(cloud_space->points[i].x)) { return caseinfo; }
	}

	//获得抓取点的长  宽  X  Y  Z
	int dis1, dis2, dis3;
	dis1 = int(qSqrt(qPow(cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(1).x, 2) + qPow(cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(1).y, 2)));
	dis2 = int(qSqrt(qPow(cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(2).x, 2) + qPow(cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(2).y, 2)));
	dis3 = int(qSqrt(qPow(cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(3).x, 2) + qPow(cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(3).y, 2)));

	caseinfo.SkuWidth = std::min(std::min(dis1, dis2), dis3);
	caseinfo.SkuLength = dis1 + dis2 + dis3 - std::max(std::max(dis1, dis2), dis3) - caseinfo.SkuWidth;

	float xx = 0;
	float yy = 0;
	for (int i = 0; i < cloud_caseconer_world->size(); i++) {
		xx += cloud_caseconer_world->at(i).x;
		yy += cloud_caseconer_world->at(i).y;
	}

	caseinfo.Position.SkuPosX = int(OBBPosition.x);
	caseinfo.Position.SkuPosY = int(OBBPosition.y);
	caseinfo.Position.SkuPosZ = int(OBBPosition.z);
	caseinfo.cloud = cloud_caseconer_world;

	//角度范围
	//考虑 cv:: fastatanfast   abs  保留第一象限角度值
	if (dis1 == caseinfo.SkuLength) {
		if (cloud_caseconer_world->at(0).x == cloud_caseconer_world->at(1).x) {
			caseinfo.Position.SkuPosZA = 90;
		} else {
			caseinfo.Position.SkuPosZA = 180 * qAtan((cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(1).y) / (cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(1).x)) / M_PI;
		}
	} else if (dis2 == caseinfo.SkuLength) {
		if (cloud_caseconer_world->at(0).x == cloud_caseconer_world->at(2).x) {
			caseinfo.Position.SkuPosZA = 90;
		} else {
			caseinfo.Position.SkuPosZA = 180 * qAtan((cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(2).y) / (cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(2).x)) / M_PI;
		}
	} else {
		if (cloud_caseconer_world->at(0).x == cloud_caseconer_world->at(3).x) {
			caseinfo.Position.SkuPosZA = 90;
		} else {
			caseinfo.Position.SkuPosZA = 180 * qAtan((cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(3).y) / (cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(3).x)) / M_PI;
		}
	}
	caseinfo.Position.SkuPosXA = 0;
	caseinfo.Position.SkuPosYA = 0;

	return caseinfo;
}

BOXINFO AlgorithmUnstackCommon::GetCaseInfoFromPix2(PointCloud::Ptr cloud_case, RECGRESULT recg)
{
	//初始化各个参数
	BOXINFO caseinfo;
	caseinfo.SkuLength = 0;
	caseinfo.SkuWidth = 0;
	caseinfo.Position = { "", 1,1,1,1,1,1 };

	//单位换算
	for (int i = 0; i < cloud_case->points.size(); i++) {
		cloud_case->points[i].z = cloud_case->points[i].z * 1000;
	}

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	// Create the segmentation object
	pcl::SACSegmentation<PointT> seg;
	PointCloud::Ptr cloud_plane(new PointCloud);
	// Optional
	seg.setOptimizeCoefficients(true);
	// Mandatory
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(500);
	seg.setDistanceThreshold(5);//单位mm
	seg.setInputCloud(cloud_case);
	seg.segment(*inliers, *coefficients);
	// 提取索引
	pcl::ExtractIndices<PointT> extract;
	// Extract the inliers
	extract.setInputCloud(cloud_case);
	extract.setIndices(inliers);       // 设置分割后的内点为需要提取的点集
	extract.setNegative(false);        // 设置提取内点
	extract.filter(*cloud_plane);          // 提取并保存至 cloud_p


	//计算质心
	Eigen::Vector4f centroid;  //质心
	pcl::compute3DCentroid(*cloud_plane, centroid);


	//计算平均高度
	double h = 0;
	for (int i = 0; i < cloud_plane->points.size(); i++) {
		h += cloud_plane->points[i].z;
	}
	h = h / cloud_plane->points.size();


	//构建识别区域点云   添加高度
	//转化到机械手坐标系下
	PointCloud::Ptr cloud_caseconer_pix(new PointCloud);
	cloud_caseconer_pix->push_back(PointT(recg.P1.x(), recg.P1.y(), h));
	cloud_caseconer_pix->push_back(PointT(recg.P2.x(), recg.P2.y(), h));
	cloud_caseconer_pix->push_back(PointT(recg.P3.x(), recg.P3.y(), h));
	cloud_caseconer_pix->push_back(PointT(recg.P4.x(), recg.P4.y(), h));//centroid[2]
	PointCloud::Ptr cloud_caseconer_world = pix2world(cloud_caseconer_pix);

	//平面点云最小包围盒及位姿
	pcl::PointXYZ OBBPosition;
	Eigen::Matrix3f OBBRotationalMatrix_;
	Eigen::Vector3f OBBSize;
	pcl::PointXYZ  MassCenter;
	pclClass.PlaneOBB(cloud_caseconer_world, &OBBPosition, &OBBRotationalMatrix_, &OBBSize, &MassCenter);

	//获得抓取点的长  宽  X  Y  Z
	int dis1, dis2, dis3;
	dis1 = int(qSqrt(qPow(cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(1).x, 2) + qPow(cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(1).y, 2)));
	dis2 = int(qSqrt(qPow(cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(2).x, 2) + qPow(cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(2).y, 2)));
	dis3 = int(qSqrt(qPow(cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(3).x, 2) + qPow(cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(3).y, 2)));

	caseinfo.SkuWidth = std::min(std::min(dis1, dis2), dis3);
	caseinfo.SkuLength = dis1 + dis2 + dis3 - std::max(std::max(dis1, dis2), dis3) - caseinfo.SkuWidth;

	float xx = 0;
	float yy = 0;
	for (int i = 0; i < cloud_caseconer_world->size(); i++) {
		xx += cloud_caseconer_world->at(i).x;
		yy += cloud_caseconer_world->at(i).y;
	}

	caseinfo.Position.SkuPosX = int(OBBPosition.x);
	caseinfo.Position.SkuPosY = int(OBBPosition.y);
	caseinfo.Position.SkuPosZ = int(OBBPosition.z);

	//caseinfo.Position.SkuPosX = int(xx / 4);
	//caseinfo.Position.SkuPosY = int(yy / 4);
	//caseinfo.Position.SkuPosZ = int(cloud_caseconer_world->at(0).z);

	//角度范围？？
	//考虑 cv:: fastatanfast   abs  保留第一象限角度值
	if (dis1 == caseinfo.SkuLength) {
		if (cloud_caseconer_world->at(0).x == cloud_caseconer_world->at(1).x) {
			caseinfo.Position.SkuPosZA = 90;
		} else {
			caseinfo.Position.SkuPosZA = 180 * qAtan((cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(1).y) / (cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(1).x)) / M_PI;
		}
	} else if (dis2 == caseinfo.SkuLength) {
		if (cloud_caseconer_world->at(0).x == cloud_caseconer_world->at(2).x) {
			caseinfo.Position.SkuPosZA = 90;
		} else {
			caseinfo.Position.SkuPosZA = 180 * qAtan((cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(2).y) / (cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(2).x)) / M_PI;
		}
	} else {
		if (cloud_caseconer_world->at(0).x == cloud_caseconer_world->at(3).x) {
			caseinfo.Position.SkuPosZA = 90;
		} else {
			caseinfo.Position.SkuPosZA = 180 * qAtan((cloud_caseconer_world->at(0).y - cloud_caseconer_world->at(3).y) / (cloud_caseconer_world->at(0).x - cloud_caseconer_world->at(3).x)) / M_PI;
		}
	}

	caseinfo.Position.SkuPosZ -= 30;

	return caseinfo;
}

PointCloud::Ptr AlgorithmUnstackCommon::Filter(PointCloud::Ptr cloud_in)
{
	//降采样
	PointCloud::Ptr cloud_lowresolution_ptr(new PointCloud);
	pcl::VoxelGrid<PointT> vg_filter;
	vg_filter.setInputCloud(cloud_in);
	vg_filter.setLeafSize(5.0f, 5.0f, 10.0f);
	vg_filter.filter(*cloud_lowresolution_ptr);

	//离群点去除
	PointCloud::Ptr cloud_filter_ptr(new PointCloud);
	pcl::StatisticalOutlierRemoval <pcl::PointXYZ> filt;
	filt.setInputCloud(cloud_lowresolution_ptr);
	filt.setMeanK(1);
	filt.setStddevMulThresh(10.0);
	filt.filter(*cloud_filter_ptr);

	return cloud_filter_ptr;
}

QVector<PointCloud::Ptr> AlgorithmUnstackCommon::GetCaseCloudVec(PointCloud::Ptr cloud)
{

	//输入点云  
	//根据识别区域  切割点云
	//输出各个区域对应的点云


	//新点云   改变Z的单位
	PointCloud::Ptr cloudin(new PointCloud);
	for (int i = 0; i < cloud->size(); i++) {
		PointT point_tempa;
		point_tempa.x = cloud->at(i).x;
		point_tempa.y = cloud->at(i).y;
		point_tempa.z = cloud->at(i).z / 1000.0;
		cloudin->push_back(point_tempa);
	}

	//根据识别结果  创建以坐标原点为中心的空间立方体
	QVector<PointCloud::Ptr> recog_vec;
	for (int i = 0; i < recog_result.size(); i++) {
		PointCloud::Ptr convex(new PointCloud);
		convex->push_back(PointT(recog_result[i].P1.x(), recog_result[i].P1.y(), 0));
		convex->push_back(PointT(recog_result[i].P2.x(), recog_result[i].P2.y(), 0));
		convex->push_back(PointT(recog_result[i].P3.x(), recog_result[i].P3.y(), 0));
		convex->push_back(PointT(recog_result[i].P4.x(), recog_result[i].P4.y(), 0));

		//convex->push_back(PointT(10, 10, 10000));
		//convex->push_back(PointT(950, 10, 10000));
		//convex->push_back(PointT(10, 950, 10000));
		//convex->push_back(PointT(950, 950, 10000));

		//ConvexHull计算凸包点
		//setInputCloud  输入需要计算的点云
		//setDimension   设置凸包维度 		
		//surface_hull   所有凸包多边形的顶点
		//polygons       所有凸包多边形的顶点在surface_hull中的下标

		pcl::ConvexHull<PointT> hull;
		hull.setInputCloud(convex);
		hull.setDimension(2);
		std::vector<pcl::Vertices> polygons;
		PointCloud::Ptr surface_hull(new PointCloud);
		hull.reconstruct(*surface_hull, polygons);

		/*pcl::visualization::PCLVisualizer viewer("haha");

		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 255, 0, 0);
		viewer.addPointCloud<pcl::PointXYZ>(cloud,single_color,"11");*/
		//viewer.addCoordinateSystem();

		// 设置点云大小
		//viewer.addPolygon<pcl::PointXYZ>(surface_hull, 0, .069 * 255, 0.2 * 255, "backview_hull_polyline1");//添加多变形
		//while (!viewer.wasStopped()) {
		//	viewer.spinOnce(1000);
		//}
		//system("pause");


		//CropHull滤波器
		//得到2D封闭多边形内部或者外部的点云；
		//setInputCloud  输入需要计算的点云
		//setDimension   设置凸包维度 		
		//setHullIndices 输入封闭多边形的顶点
		//setHullCloud   输入封闭多边形的形状
		//setDim         设置维度：该维度需要与凸包维度一致

		PointCloud::Ptr temp(new PointCloud);
		pcl::CropHull<PointT> ch_filter;
		ch_filter.setInputCloud(cloudin);
		ch_filter.setDim(2);
		ch_filter.setHullIndices(polygons);
		ch_filter.setHullCloud(surface_hull);
		ch_filter.filter(*temp);

		recog_vec.push_back(temp);
	}

	return recog_vec;
}

PointCloud::Ptr AlgorithmUnstackCommon::GetCloudWithoutCase(BOXINFO caseinfo, PointCloud::Ptr cloud_ptr_world)
{
	PointCloud::Ptr cloud_check_ptr(new PointCloud);

	Eigen::Vector3f minpoint, maxpoint;
	float temppx = float(caseinfo.Position.SkuPosX) / 1000;
	float temppy = float(caseinfo.Position.SkuPosY) / 1000;
	float temppz = float(caseinfo.Position.SkuPosZ) / 1000;
	float radian = caseinfo.Position.SkuPosZA * M_PI / 180.0;
	minpoint.x() = caseinfo.Position.SkuPosX - float(caseinfo.SkuLength) / 2 - EXTEND/* / 2 * qCos(radian) - float(caseinfo.SkuWidth) / 2 * qSin(radian)*/;
	minpoint.y() = caseinfo.Position.SkuPosY - float(caseinfo.SkuWidth) / 2 - EXTEND/* / 2 * qCos(radian) + float(caseinfo.SkuLength) / 2 * qSin(radian)*/;
	minpoint.z() = caseinfo.Position.SkuPosZ - 100;
	maxpoint.x() = caseinfo.Position.SkuPosX + float(caseinfo.SkuLength) / 2 + EXTEND/* / 2 * qCos(radian) + float(caseinfo.SkuWidth) / 2 * qSin(radian)*/;
	maxpoint.y() = caseinfo.Position.SkuPosY + float(caseinfo.SkuWidth) / 2 + EXTEND/* / 2 * qCos(radian) - float(caseinfo.SkuLength) / 2 * qSin(radian)*/;
	maxpoint.z() = caseinfo.Position.SkuPosZ + 100;
	minpoint.x() = minpoint.x() / 1000.0;
	minpoint.y() = minpoint.y() / 1000.0;
	minpoint.z() = minpoint.z() / 1000.0;
	maxpoint.x() = maxpoint.x() / 1000.0;
	maxpoint.y() = maxpoint.y() / 1000.0;
	maxpoint.z() = maxpoint.z() / 1000.0;
	Eigen::Vector3f tra1;
	Eigen::Vector3f tra2{ temppx, temppy, temppz };
	Eigen::Vector3f tra3;
	Eigen::Vector3f rot{ 0, 0, radian };
	Eigen::AngleAxisf Rotation_vector(radian, Eigen::Vector3f(0, 0, 1));
	Eigen::Matrix3f Rotation_Mateix = Rotation_vector.matrix();
	tra1 = Rotation_Mateix * tra2;
	tra3 = tra2 - tra1;
	pcl::CropBox<PointT> cb_filter;
	cb_filter.setMin(Eigen::Vector4f(minpoint.x(), minpoint.y(), minpoint.z(), 1.0));
	cb_filter.setMax(Eigen::Vector4f(maxpoint.x(), maxpoint.y(), maxpoint.z(), 1.0));
	cb_filter.setInputCloud(cloud_ptr_world);
	cb_filter.setKeepOrganized(false); //default:remove point,when true refine points to Nan
	cb_filter.setNegative(true);
	cb_filter.setRotation(rot);
	cb_filter.setTranslation(tra3);
	//对这个box进行旋转
	//cb_filter.setTranslation(tra2);
	cb_filter.filter(*cloud_check_ptr);

	return cloud_check_ptr;

}

PointCloud::Ptr AlgorithmUnstackCommon::GetCloudWithoutCaseVec(QVector<BOXINFO> caseinfo_vec, PointCloud::Ptr cloud_ptr_world)
{
	PointCloud::Ptr cloud_check_ptr(new PointCloud);
	cloud_check_ptr = cloud_ptr_world;
	for (int i = 0; i < caseinfo_vec.size(); i++) {
		cloud_check_ptr = GetCloudWithoutCase(caseinfo_vec[i], cloud_check_ptr);
	}
	return cloud_check_ptr;
}

PointCloud::Ptr AlgorithmUnstackCommon::GetCloudWithCase(BOXINFO caseinfo, PointCloud::Ptr cloud_ptr_world)
{
	PointCloud::Ptr cloud_check_ptr(new PointCloud);
	float radian = (caseinfo.Position.SkuPosZA) * M_PI / 180.0;
	for (int i = 0; i < 200; i++) {
		for (int j = 0; j < 200; j++) {
			PointT temppoint;
			temppoint.x = (-float(caseinfo.SkuLength) / 2. + float(caseinfo.SkuLength) / 200. * j) / 1000.0;
			temppoint.y = (-float(caseinfo.SkuWidth) / 2. + float(caseinfo.SkuWidth) / 200. * i) / 1000.0;
			temppoint.z = 0.;
			cloud_check_ptr->points.push_back(temppoint);
		}
	}
	Eigen::Affine3f transform = Eigen::Affine3f::Identity();
	transform.translation() << float(caseinfo.Position.SkuPosX) / 1000.0, float(caseinfo.Position.SkuPosY) / 1000.0, float(caseinfo.Position.SkuPosZ) / 1000.0;
	transform.rotate(Eigen::AngleAxisf(radian, Eigen::Vector3f::UnitZ()));
	PointCloud::Ptr cloud_suckerconvex_world(new PointCloud);
	pcl::transformPointCloud(*cloud_check_ptr, *cloud_check_ptr, transform);
	return cloud_check_ptr;
}

PointCloud::Ptr AlgorithmUnstackCommon::GetCloudWithCaseVec(QVector<BOXINFO> caseinfo_vec, PointCloud::Ptr cloud_ptr_world)
{
	PointCloud::Ptr cloud_check_ptr(new PointCloud);
	for (int i = 0; i < caseinfo_vec.size(); i++) {
		PointCloud::Ptr temp(new PointCloud);
		temp = GetCloudWithCase(caseinfo_vec[i], cloud_ptr_world);
		cloud_check_ptr->points.insert(cloud_check_ptr->points.end(), temp->points.begin(), temp->points.end());
	}
	return cloud_check_ptr;
}

int AlgorithmUnstackCommon::GetTopFloorCase(QVector<BOXINFO>& case_vec)
{
	float topheight = case_vec.at(0).Position.SkuPosZ;
	for (int i = 0; i < case_vec.size(); i++) {
		if (abs(case_vec[i].Position.SkuPosZ - topheight) > UNSTACK_LAYER_GAP_2) {
			case_vec.erase(case_vec.begin() + i, case_vec.end());
			break;
		}
	}

	if (case_vec.size() == 0) {
		return 0;
	}

	return 1;
}

int AlgorithmUnstackCommon::PointCloudCal(BOXINFO caseinfo, PointCloud::Ptr point_check_ptr, SKUPOSITION& res, cv::Mat& image)
{
	//point_check_ptr:扣完箱子之后的场景电点云
	float delta_x = 0, delta_y = 0;
	PointCloud::Ptr sucker_convex(new PointCloud);
	sucker_convex = GetJigPoint();
	if (PointCloudInterferenceCheck(caseinfo, sucker_convex, point_check_ptr, delta_x, delta_y, image)) {
		//校验干涉  
		res.SkuPosX = caseinfo.Position.SkuPosX + delta_x;
		res.SkuPosY = caseinfo.Position.SkuPosY + delta_y;
		res.SkuPosZ = caseinfo.Position.SkuPosZ;
		res.SkuPosXA = 180;
		res.SkuPosYA = 0;
		res.SkuPosZA = caseinfo.Position.SkuPosZA;
		res.SkuPosZA = res.SkuPosZA;
		return 1;
	}
	return 0;
}

QVector<QVector<BOXINFO>> AlgorithmUnstackCommon::FindNeatCase(QVector<BOXINFO> const case_vec)
{
	QVector<BOXINFO> case_O;//水平 0度
	QVector<BOXINFO> case_P;//竖直
	QVector<BOXINFO> case_X;//倾斜
	QVector<QVector<BOXINFO> > res;

	//将所有的点分类  分为三部分
	for (int i = 0; i < case_vec.size(); i++) {
		if (abs(case_vec[i].Position.SkuPosZA - 90) < UNSTACK_ANGLE_FIVE || abs(case_vec[i].Position.SkuPosZA + 90) < UNSTACK_ANGLE_FIVE) {
			case_P.push_back(case_vec[i]);
		}

		else if (abs(case_vec[i].Position.SkuPosZA) < UNSTACK_ANGLE_FIVE) {
			case_O.push_back(case_vec[i]);
		}

		else {
			case_X.push_back(case_vec[i]);
		}
	}

	//获取水平方向相邻的箱体
	if (relativeX == 1)
		std::sort(case_O.begin(), case_O.end(), compbyXP);
	else if (relativeX == 0)
		std::sort(case_O.begin(), case_O.end(), compbyXN);

	QVector<int> indexA;
	if (case_O.size() > 0) {
		indexA.push_back(0);
		for (int i = 0; i < case_O.size() - 1; i++) {
			if (abs(case_O[i].Position.SkuPosX - case_O[i + 1].Position.SkuPosX) > case_O[i].SkuLength / 4)
				indexA.push_back(i + 1);
		}
		indexA.push_back(case_O.size());
	}

	QVector <QVector<BOXINFO>> case_ONeat;
	for (int i = 0; i < indexA.size() - 1; i++) {
		QVector<BOXINFO> temp1(case_O.begin() + indexA.at(i), case_O.begin() + indexA.at(i + 1));
		if (relativeY == 1)
			std::sort(temp1.begin(), temp1.end(), compbyYP);
		else if (relativeY == 0)
			std::sort(temp1.begin(), temp1.end(), compbyYN);
		case_ONeat.push_back(temp1);
	}

	//获取垂直方向相邻的两个箱体
	if (relativeY == 1)
		std::sort(case_P.begin(), case_P.end(), compbyYP);
	else if (relativeY == 0)
		std::sort(case_P.begin(), case_P.end(), compbyYN);

	QVector<int> indexB;
	if (case_P.size() > 0) {
		indexB.push_back(0);
		for (int i = 0; i < case_P.size() - 1; i++) {
			if (abs(abs(case_P[i].Position.SkuPosY - case_P[i + 1].Position.SkuPosY) > case_P[i].SkuWidth / 4))
				indexB.push_back(i + 1);
		}
		indexB.push_back(case_P.size());
	}

	QVector <QVector<BOXINFO>> case_PNeat;
	for (int i = 0; i < indexB.size() - 1; i++) {
		QVector<BOXINFO> temp1(case_P.begin() + indexB.at(i), case_P.begin() + indexB.at(i + 1));
		if (relativeX == 1)
			std::sort(temp1.begin(), temp1.end(), compbyXP);
		else if (relativeX == 0)
			std::sort(temp1.begin(), temp1.end(), compbyXN);
		case_PNeat.push_back(temp1);
	}

	for (int i = 0; i < case_ONeat.size(); i++) {
		res.push_back(case_ONeat[i]);
	}
	for (int i = 0; i < case_PNeat.size(); i++) {
		res.push_back(case_PNeat[i]);
	}
	for (int i = 0; i < case_X.size(); i++) {
		res.push_back({ case_X.at(i) });
	}
	return res;
}

bool AlgorithmUnstackCommon::SetUnstackSetting(const visAlgParam& param)
{
	//获取拆垛方向
	m_dir = param.dir;
	switch (param.dir) {
	case 0:
		relativeX = 1;
		relativeY = 1;
		break;
	case 1:
		relativeX = 0;
		relativeY = 1;
		break;
	case 2:
		relativeX = 0;
		relativeY = 0;
		break;
	case 3:
		relativeX = 1;
		relativeY = 0;
		break;
	default:

		return false;
	}

	//获取吸盘安装方式
	angleX = 90;
	angleY = 0;
	//最多抓取个数
	GrabNum = param.maxNum;
	if (GrabNum <= 0)
		return false;
	//获取是否开启尺寸校验
	SizeCheck = true;
	//获取尺寸范围信息
	maxBoxLength = 1000;
	maxBoxWidth = 1000;
	minBoxLength = 100;
	minBoxWidth = 100;
	return true;
}

bool AlgorithmUnstackCommon::SetIntrinsic(QString ins)
{
	intrinsic_matrix.clear();
	intrinsicMatrix = cv::Mat::eye(3, 3, CV_32FC1);//内参
	distortionMatrix = cv::Mat::zeros(1, 5, CV_32FC1);//畸变系数
	QFile calibfile(ins);
	calibfile.open(QIODevice::ReadOnly | QIODevice::Text);
	QString value = calibfile.readAll();
	calibfile.close();

	QJsonDocument doc(QJsonDocument::fromJson(value.toUtf8()));
	if (doc.isObject()) {
		if (doc.object().contains(CALIBRATION_INS)) {
			QJsonObject obj = doc[CALIBRATION_INS].toObject();
			intrinsic_matrix.push_back(QVector<float>{float(obj[INTRINSIC_FX].toDouble()), 0.0, float(obj[INTRINSIC_CX].toDouble())});
			intrinsic_matrix.push_back(QVector<float>{0, float(obj[INTRINSIC_FY].toDouble()), float(obj[INTRINSIC_CY].toDouble())});
			intrinsic_matrix.push_back(QVector<float>{0, 0, 1});
			intrinsicMatrix.at<float>(0, 0) = float(obj[INTRINSIC_FX].toDouble());
			intrinsicMatrix.at<float>(0, 2) = float(obj[INTRINSIC_CX].toDouble());
			intrinsicMatrix.at<float>(1, 1) = float(obj[INTRINSIC_FY].toDouble());
			intrinsicMatrix.at<float>(1, 2) = float(obj[INTRINSIC_CY].toDouble());
			return true;
		}
		return false;
	}

	return false;
}

bool AlgorithmUnstackCommon::SetExternal(QString ext)
{
	external_matrix.clear();
	externalMatrix = cv::Mat::eye(4, 4, CV_64FC1);//外参
	QFile calibfile(ext);
	calibfile.open(QIODevice::ReadOnly | QIODevice::Text);
	QString value = calibfile.readAll();
	calibfile.close();

	QJsonDocument doc(QJsonDocument::fromJson(value.toUtf8()));
	if (doc.isObject()) {
		if (doc.object().contains(CALIBRATION_EXT)) {
			QJsonObject obj = doc[CALIBRATION_EXT].toObject();
			external_matrix.push_back(QVector<float>{float(obj[EXTERNAL_00].toDouble()), float(obj[EXTERNAL_01].toDouble()), float(obj[EXTERNAL_02].toDouble()), float(obj[EXTERNAL_03].toDouble())});
			external_matrix.push_back(QVector<float>{float(obj[EXTERNAL_10].toDouble()), float(obj[EXTERNAL_11].toDouble()), float(obj[EXTERNAL_12].toDouble()), float(obj[EXTERNAL_13].toDouble())});
			external_matrix.push_back(QVector<float>{float(obj[EXTERNAL_20].toDouble()), float(obj[EXTERNAL_21].toDouble()), float(obj[EXTERNAL_22].toDouble()), float(obj[EXTERNAL_23].toDouble())});
			external_matrix.push_back(QVector<float>{0, 0, 0, 1});
			externalMatrix.at<double>(0, 0) = float(obj[EXTERNAL_00].toDouble());
			externalMatrix.at<double>(0, 1) = float(obj[EXTERNAL_01].toDouble());
			externalMatrix.at<double>(0, 2) = float(obj[EXTERNAL_02].toDouble());
			externalMatrix.at<double>(0, 3) = float(obj[EXTERNAL_03].toDouble());
			externalMatrix.at<double>(1, 0) = float(obj[EXTERNAL_10].toDouble());
			externalMatrix.at<double>(1, 1) = float(obj[EXTERNAL_11].toDouble());
			externalMatrix.at<double>(1, 2) = float(obj[EXTERNAL_12].toDouble());
			externalMatrix.at<double>(1, 3) = float(obj[EXTERNAL_13].toDouble());
			externalMatrix.at<double>(2, 0) = float(obj[EXTERNAL_20].toDouble());
			externalMatrix.at<double>(2, 1) = float(obj[EXTERNAL_21].toDouble());
			externalMatrix.at<double>(2, 2) = float(obj[EXTERNAL_22].toDouble());
			externalMatrix.at<double>(2, 3) = float(obj[EXTERNAL_23].toDouble());
			return true;
		}
		return false;
	}
	return false;
}

bool AlgorithmUnstackCommon::SetWorkSpace(QString space)
{
	QFile calibfile(space);
	calibfile.open(QIODevice::ReadOnly | QIODevice::Text);
	QString value = calibfile.readAll();
	calibfile.close();

	QJsonDocument doc(QJsonDocument::fromJson(value.toUtf8()));
	if (doc.isObject()) {
		if (doc.object().contains(UNSTACKSPACE_LIMITS)) {
			QJsonObject obj = doc[UNSTACKSPACE_LIMITS].toObject();
			xlimit_max = obj["x_max"].toDouble();
			xlimit_min = obj["x_min"].toDouble();
			ylimit_max = obj["y_max"].toDouble();
			ylimit_min = obj["y_min"].toDouble();
			zlimit_max = obj["z_max"].toDouble();
			zlimit_min = obj["z_min"].toDouble();
			return true;
		}
		return false;
	}
	return false;
}

void AlgorithmUnstackCommon::UpdateBeforeinfo(BOXINFO reinfo, int dx, int dy, RECORDINFO& info)
{
	if (info.btrue == false) {
		info.btrue = true;
		info.z = reinfo.Position.SkuPosZ;
		info.za = reinfo.Position.SkuPosZA;

		info.dx = dx;
		info.dy = dy;


		for (size_t n = 0; n < recog_result.size(); n++) {
			if (reinfo.id == recog_result[n].id) {
				info.sult.P1.setX(recog_result[n].P1.x());
				info.sult.P1.setY(recog_result[n].P1.y());

				info.sult.P2.setX(recog_result[n].P2.x());
				info.sult.P2.setY(recog_result[n].P2.y());

				info.sult.P3.setX(recog_result[n].P3.x());
				info.sult.P3.setY(recog_result[n].P3.y());

				info.sult.P4.setX(recog_result[n].P4.x());
				info.sult.P4.setY(recog_result[n].P4.y());

				break;
			}
		}

	}
}

void AlgorithmUnstackCommon::CalPointDifference(QString file, RECORDINFO info, QString& ipt)
{
	//创建点云
	PointCloud::Ptr cloud_input_ptr(new PointCloud);//此为相机系
	cv::Mat depthori = cv::imread(file.toStdString(), -1);
	int h = depthori.rows;
	int w = depthori.cols;
	cv::Mat gray;
	if (depthori.type() == CV_16UC1) {
		depthori.convertTo(depthori, CV_32FC1);
		gray = depthori;
	} else {
		cv::Mat depth_float(depthori.rows, depthori.cols, CV_32FC1, depthori.data);//转32位图像
		gray = depth_float;
	}

	for (int x = 0; x < h; x++) {
		for (int y = 0; y < w; y++) {
			PointT point_temp;
			float zz = gray.at<float>(x, y);
			point_temp.z = zz;
			point_temp.x = y;
			point_temp.y = x;

			if (point_temp.z <= 5000 && zz > 0) {
				cloud_input_ptr->points.push_back(point_temp);
			}

		}
	}
	cloud_input_ptr->height = 1;
	cloud_input_ptr->width = cloud_input_ptr->points.size();

	//滤波d
	PointCloud::Ptr cloud_filter_ptr = Filter(cloud_input_ptr);

	RECGRESULT  st;

	st.P1.setX(info.sult.P1.x());
	st.P1.setY(info.sult.P1.y());

	st.P2.setX(info.sult.P2.x());
	st.P2.setY(info.sult.P2.y());

	st.P3.setX(info.sult.P3.x());
	st.P3.setY(info.sult.P3.y());

	st.P4.setX(info.sult.P4.x());
	st.P4.setY(info.sult.P4.y());


	//创建识别结果子点云，用于求识别结果空间位姿。

	PointCloud::Ptr recog_cloud_ptr_vec = GetCaseCloud(cloud_filter_ptr, st);

	//坐标系转换
	PointCloud::Ptr cloud_ptr_world(new PointCloud);
	cloud_ptr_world = pix2world(cloud_filter_ptr);
	cloud_ptr_world->height = 1;
	cloud_ptr_world->width = cloud_ptr_world->points.size();


	for (int i = 0; i < cloud_ptr_world->size(); i++) {
		cloud_ptr_world->at(i).x = cloud_ptr_world->at(i).x / 1000.0;
		cloud_ptr_world->at(i).y = cloud_ptr_world->at(i).y / 1000.0;
		cloud_ptr_world->at(i).z = cloud_ptr_world->at(i).z / 1000.0;
	}

	BOXINFO tempcase = GetCaseInfoFromPix2(recog_cloud_ptr_vec, st);

	// 构建 JSON 对象
	QJsonObject json;
	json.insert("DeltaX", info.dx);
	json.insert("DeltaY", info.dy);
	json.insert("PositionZA", info.za);
	json.insert("DeltaZ", (info.z - tempcase.Position.SkuPosZ));

	// 构建 JSON 文档
	QJsonDocument document;
	document.setObject(json);
	QByteArray byteArray = document.toJson(QJsonDocument::Compact);
	QString strJson(byteArray);
	ipt = strJson;

	beforinfo.btrue = false;
}

PointCloud::Ptr AlgorithmUnstackCommon::GetCaseCloud(PointCloud::Ptr cloud, RECGRESULT result)
{
	PointCloud::Ptr cloudin(new PointCloud);
	for (int i = 0; i < cloud->size(); i++) {
		PointT point_tempa;
		point_tempa.x = cloud->at(i).x;
		point_tempa.y = cloud->at(i).y;
		point_tempa.z = cloud->at(i).z / 1000.0;
		cloudin->push_back(point_tempa);
	}
	cloudin->height = 1;
	cloudin->width = cloudin->points.size();
	//pcl::io::savePCDFile("cloudin.pcd", *cloudin);


	PointCloud::Ptr convex(new PointCloud);
	convex->push_back(PointT(result.P1.x(), result.P1.y(), 0));
	convex->push_back(PointT(result.P2.x(), result.P2.y(), 0));
	convex->push_back(PointT(result.P3.x(), result.P3.y(), 0));
	convex->push_back(PointT(result.P4.x(), result.P4.y(), 0));

	pcl::ConvexHull<PointT> hull;
	hull.setInputCloud(convex);
	hull.setDimension(2);
	std::vector<pcl::Vertices> polygons;
	PointCloud::Ptr surface_hull(new PointCloud);
	hull.reconstruct(*surface_hull, polygons);

	PointCloud::Ptr temp(new PointCloud);
	pcl::CropHull<PointT> ch_filter;
	ch_filter.setInputCloud(cloudin);
	ch_filter.setDim(2);
	ch_filter.setHullIndices(polygons);
	ch_filter.setHullCloud(surface_hull);
	ch_filter.filter(*temp);

	temp->height = 1;
	temp->width = temp->points.size();
	//pcl::io::savePCDFile("temp.pcd", *temp);



	return temp;
}

//欧式聚类提取 （欧几里得聚类提取）
void AlgorithmUnstackCommon::EuclideanClusterExtractionCloud(const PointCloud::Ptr& pCloud, float fClusterTolerance, int iMinClusterSize, int iMaxClusterSize, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>* pvCloudPtr)
{
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(pCloud);

	std::vector<pcl::PointIndices> cluster_indices;//提取的聚类
	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;//欧式聚类提取 （欧几里得聚类提取）
	ec.setClusterTolerance(fClusterTolerance); //簇的容差 2cm
	ec.setMinClusterSize(iMinClusterSize);//最小簇
	ec.setMaxClusterSize(iMaxClusterSize);//最大簇
	ec.setSearchMethod(tree);//kd树查找模式
	ec.setInputCloud(pCloud);
	ec.extract(cluster_indices);//提取

	//遍历每一个聚类
	int j = 0;
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloudPtr;
	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
			cloud_cluster->points.push_back(pCloud->points[*pit]);
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;

		if (cloud_cluster->points.size() < 300)
			continue;
		vCloudPtr.push_back(cloud_cluster);
		j++;
	}
	*pvCloudPtr = vCloudPtr;

}

double AlgorithmUnstackCommon::getMaxCloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloudPtr)
{
	//最大点云
	int iMaxCloudIndex = 0;
	double iMaxSize = -10000;
	for (int i = 0; i < vCloudPtr.size(); i++) {
		pcl::PointCloud<pcl::PointXYZ>::Ptr temp = vCloudPtr.at(i);

		//剪切特定方向点云 //根据向量方向进行裁剪
		pclClass.CropCloudNormalRange(temp, 40, temp, 'z');

		//平面分割
		pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudPlane;
		pcl::ModelCoefficients::Ptr pcoefficients;
		pclClass.PlaneSegmentation(temp, 0.02, &pcoefficients, &pCloudPlane); //平面分割参数设置为2cm

		if (pCloudPlane->points.size() < 300) {
			continue;
		}

		//计算平面角度
		float fAlpha, fBeta, fGamma, fDistance;
		pclClass.PlaneCoefficients2Euler(pcoefficients, &fAlpha, &fBeta, &fGamma, &fDistance);

		//todo  fGamma验证点云方向
		//-------------------------
		float anglex = 30;
		if (!(abs(fGamma) < anglex || abs(fGamma - 180) < anglex || abs(fGamma + 180) < anglex)) {
			continue;
		}
		//-------------------------

		//计算平均高度
		double h = 0;
		for (int j = 0; j < pCloudPlane->points.size(); j++) {
			h += pCloudPlane->points[j].z;
		}
		h = h / pCloudPlane->points.size();
		if (h > iMaxSize) {
			iMaxSize = h;
		}
	}
	return iMaxSize * 1000;
}

PointCloud::Ptr AlgorithmUnstackCommon::cam2pix(PointCloud::Ptr cloud)
{
	PointCloud::Ptr cloud_copy(new PointCloud);
	cloud_copy->points.resize(cloud->points.size());
	for (int i = 0; i < cloud->points.size(); i++) {
		cloud_copy->points.at(i).x = (cloud->points.at(i).x * intrinsic_matrix[0][0]) / cloud->points.at(i).z + intrinsic_matrix[0][2];
		cloud_copy->points.at(i).y = (cloud->points.at(i).y * intrinsic_matrix[1][1]) / cloud->points.at(i).z + intrinsic_matrix[1][2];
		cloud_copy->at(i).z = cloud->at(i).z;
	}
	cloud_copy->width = cloud_copy->points.size();
	cloud_copy->height = 1;
	return cloud_copy;
}

PointCloud::Ptr AlgorithmUnstackCommon::pix2cam(PointCloud::Ptr cloud)
{
	PointCloud::Ptr cloud_copy(new PointCloud);
	cloud_copy->points.resize(cloud->points.size());
	for (int i = 0; i < cloud->points.size(); i++) {
		cloud_copy->points.at(i).x = (cloud->points.at(i).x - intrinsic_matrix[0][2]) * cloud->points.at(i).z / intrinsic_matrix[0][0];//已经乘过Z了
		cloud_copy->points.at(i).y = (cloud->points.at(i).y - intrinsic_matrix[1][2]) * cloud->points.at(i).z / intrinsic_matrix[1][1];
		cloud_copy->at(i).z = cloud->at(i).z;
	}
	cloud_copy->width = cloud_copy->points.size();
	cloud_copy->height = 1;
	return cloud_copy;
}

cv::RotatedRect AlgorithmUnstackCommon::getminAreaRect(cv::Mat Mask)
{
	if (Mask.empty()) {
		cv::RotatedRect minRect;
		return minRect;
	}

	cv::Mat canny_output = Mask;
	vector<vector<Point>> contours;//轮廓
	vector<Vec4i> hireachy;//层次结构
	findContours(canny_output, contours, hireachy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));//寻找边缘的轮廓

	//swx bug
	if (contours.size() == 0) {
		RotatedRect minRect;
		return minRect;
	}

	float maxw = 0;
	float maxh = 0;
	float maxArea = 0;//最大面积
	double degree = 0;
	int t_max = 0;//最大轮廓的序号
	vector<cv::RotatedRect> cminRect(contours.size());
	for (size_t t = 0; t < contours.size(); t++) {

		RotatedRect minRect = cv::minAreaRect(contours[t]);//轮廓最小外接矩形
		cminRect[t] = minRect;
		float area = minRect.size.width * minRect.size.height;
		if (area > maxArea && (area < 0.95 * canny_output.size().area()))//小于整张图片的面积的0.99倍 过滤最大的图片边缘
		{//寻找最大面积的外接矩形
			maxArea = area;
			t_max = t;//记录序号
		}
	}

	RotatedRect minRect = cminRect[t_max];//获取轮廓最小外接矩形
	return minRect;
}

//下采样的mask上采样
void AlgorithmUnstackCommon::UpsampleMask(cv::Mat mask, cv::Mat& upMask, int pad)
{
	upMask = mask.clone();
	for (int i = 0; i < mask.rows; i++) {
		for (int j = 0; j < mask.cols; j++) {
			if (mask.at<uchar>(i, j) != 0) {
				for (int p = -pad; p <= pad; p++) {
					for (int q = -pad; q <= pad; q++) {
						if (i + p < 0 || j + q < 0 || i + p >= mask.rows || j + q >= mask.cols) {
							continue;
						} else {
							upMask.at<uchar>(i + p, j + q) = 255;
						}
					}
				}
			}
		}
	}
	return;
}

int AlgorithmUnstackCommon::BoxCheck(BOXINFO recog, PointCloud::Ptr cloud_filter_ptr)
{
	int deltx, delty;
	if (relativeX == 0)
		deltx = -1;
	else
		deltx = 1;

	if (relativeY == 0)
		delty = -1;
	else
		delty = 1;

	int skewX = deltx * 300;
	int skewY = delty * 300;

	PointCloud::Ptr boxCloud = recog.cloud;

	int count = boxCloud->points.size();
	for (int i = 0; i < count; i++) {

		PointT point_temp;
		point_temp.z = boxCloud->points.at(i).z + 50;
		point_temp.x = boxCloud->points.at(i).x;
		point_temp.y = boxCloud->points.at(i).y;

		PointT point_temp2;
		point_temp2.z = boxCloud->points.at(i).z - 50;
		point_temp2.x = boxCloud->points.at(i).x;
		point_temp2.y = boxCloud->points.at(i).y;

		boxCloud->points.push_back(point_temp);
		boxCloud->points.push_back(point_temp2);
	}

	//箱子点云变换
	Eigen::Affine3f transform1 = Eigen::Affine3f::Identity();
	transform1.translation() << skewX, 0, 0;
	transform1.rotate(Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ()));
	PointCloud::Ptr boxCloudTrans1(new PointCloud);
	pcl::transformPointCloud(*boxCloud, *boxCloudTrans1, transform1);

	Eigen::Affine3f transform2 = Eigen::Affine3f::Identity();
	transform2.translation() << 0, skewY, 0;
	transform2.rotate(Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ()));
	PointCloud::Ptr boxCloudTrans2(new PointCloud);
	pcl::transformPointCloud(*boxCloud, *boxCloudTrans2, transform2);

	for (int i = 0; i < boxCloud->points.size(); i++) {
		boxCloud->points.at(i).x /= 1000.0;
		boxCloud->points.at(i).y /= 1000.0;
		boxCloud->points.at(i).z /= 1000.0;

		boxCloudTrans1->points.at(i).x /= 1000.0;
		boxCloudTrans1->points.at(i).y /= 1000.0;
		boxCloudTrans1->points.at(i).z /= 1000.0;

		boxCloudTrans2->points.at(i).x /= 1000.0;
		boxCloudTrans2->points.at(i).y /= 1000.0;
		boxCloudTrans2->points.at(i).z /= 1000.0;
	}

	if (bSave) {
		boxCloudTrans1->height = 1;
		boxCloudTrans1->width = boxCloudTrans1->points.size();
		boxCloudTrans2->height = 1;
		boxCloudTrans2->width = boxCloudTrans2->points.size();
		boxCloud->height = 1;
		boxCloud->width = boxCloud->points.size();
		pcl::io::savePCDFile("cloud_checkBox1.pcd", *boxCloudTrans1);
		pcl::io::savePCDFile("cloud_checkBox2.pcd", *boxCloudTrans2);
		pcl::io::savePCDFile("cloud_checkBox.pcd", *boxCloud);
	}
	return (getPointIntersection(boxCloudTrans1, cloud_filter_ptr) + getPointIntersection(boxCloudTrans2, cloud_filter_ptr));
}

bool AlgorithmUnstackCommon::GetBoxSize(QString arr)
{
	QByteArray  json = arr.toUtf8();
	QJsonDocument dd = QJsonDocument::fromJson(json);
	if (dd.isArray()) {
		QJsonArray obj = dd.array();
		if (obj.size() == 3) {
			boxLength = obj.at(0).toInt();
			boxWidth = obj.at(1).toInt();
			boxHeight = obj.at(2).toInt();
			return true;
		}
	}
	return false;
}

void AlgorithmUnstackCommon::BlockCheck(QVector<QVector<BOXINFO>>& neatcase, int casenum)
{
	//找到数量大于casenum的索引
	std::vector<int> vindex2;
	for (int i = 0; i < neatcase.size(); i++) {
		if (neatcase[i].size() > casenum) {
			vindex2.push_back(i);
		}
	}

	QVector<QVector<BOXINFO>> bedNetcases2;
	for (int i = 0; i < vindex2.size(); i++) {
		bedNetcases2.push_back(neatcase[vindex2[i]]);
	}

	//把数量多的删除  一删删一行或一列
	for (size_t q = 0; q < vindex2.size(); q++) {
		neatcase.remove(vindex2[q]);
		for (size_t p = 0; p < vindex2.size(); p++) {
			vindex2[p] -= 1;
		}
	}

	//将数量多的拆解放入netcase
	for (int i = 0; i < bedNetcases2.size(); i++) {
		//先将不能整除的放入
		int a = bedNetcases2[i].size() % casenum;
		QVector<BOXINFO> temp1;
		for (int j = 0; j < a; j++) {
			temp1.push_back(bedNetcases2[i][j]);
		}
		if (temp1.size() > 0) {
			neatcase.push_back(temp1);
		}
		//剩下的都能够被整除
		for (int j = 0; j < bedNetcases2[i].size() / casenum; j++) {
			QVector<BOXINFO> temp2;
			for (int t = 0; t < casenum; t++) {
				temp2.push_back(bedNetcases2[i][j * casenum + t + a]);
			}
			neatcase.push_back(temp2);
		}
	}


	std::vector<int> vindex;

	float deltax = 0.0;
	float deltay = 0.0;


	////箱体之间X与Y的差值是否符合多抓要求
	//for (size_t n = 0; n < neatcase.size(); n++) {
	//	for (size_t m = 0; m < neatcase[n].size() - 1; m++) {
	//		deltax = abs(neatcase[n][m].Position.SkuPosX - neatcase[n][m + 1].Position.SkuPosX);
	//		deltay = abs(neatcase[n][m].Position.SkuPosY - neatcase[n][m + 1].Position.SkuPosY);
	//		if (abs(neatcase[n][m].Position.SkuPosZA - 90) < UNSTACK_ANGLE_FIVE || abs(neatcase[n][m].Position.SkuPosZA + 90) < UNSTACK_ANGLE_FIVE) {
	//			if (deltax >= (neatcase[n][m].SkuWidth + 50) || deltay > 40) {
	//				vindex.push_back(n);
	//				break;
	//			}
	//		}

	//		else if (abs(neatcase[n][m].Position.SkuPosZA) < UNSTACK_ANGLE_FIVE) {
	//			if (deltay > (neatcase[n][m].SkuWidth + 50) || deltax > 40) {
	//				vindex.push_back(n);
	//				break;
	//			}
	//		}

	//	}
	//}


	//QVector<QVector<BOXINFO>> bedNetcases;
	//for (int i = 0; i < vindex.size(); i++) {
	//	bedNetcases.push_back(neatcase[vindex[i]]);
	//}
	////把距离过远的删除  一删删一行或一列
	//for (size_t q = 0; q < vindex.size(); q++) {
	//	neatcase.remove(vindex[q]);
	//	for (size_t p = 0; p < vindex.size(); p++) {
	//		vindex[p] -= 1;
	//	}
	//}
	////将不符合条件的拆解放入netcase
	//for (int i = 0; i < bedNetcases.size(); i++) {
	//	for (int j = 0; j < bedNetcases[i].size(); j++) {
	//		neatcase.push_back({ bedNetcases[i][j] });
	//	}
	//}

	//箱体之间X与Y的差值是否符合多抓要求
	QVector<QVector<BOXINFO>> result;
	for (int n = 0; n < neatcase.size(); n++)
	{
		QVector<BOXINFO> tmpV = neatcase.at(n);
		//先把第一个取出
		//大于三个的情况,要判断哪个和哪个对齐
		std::vector<int> index;
		index.push_back(0);
		//选出一个参照
		BOXINFO tmp = tmpV.at(0);
		for (int m = 1; m < neatcase[n].size(); m++)
		{
			deltax = abs(neatcase[n][m].Position.SkuPosX - neatcase[n][m - 1].Position.SkuPosX);
			deltay = abs(neatcase[n][m].Position.SkuPosY - neatcase[n][m - 1].Position.SkuPosY);
			//如果当前箱子不满足条件，则将不满足的的索引放入index
			if (abs(neatcase[n][m].Position.SkuPosZA - 90) < UNSTACK_ANGLE_FIVE || abs(neatcase[n][m].Position.SkuPosZA + 90) < UNSTACK_ANGLE_FIVE)
			{
				if (deltax >= (neatcase[n][m].SkuWidth + 50) || deltay > 40)
				{
					index.push_back(m);
				}
			}
			else if (abs(neatcase[n][m].Position.SkuPosZA) < UNSTACK_ANGLE_FIVE)
			{
				if (deltay > (neatcase[n][m].SkuWidth + 50) || deltax > 40)
				{
					index.push_back(m);
				}
			}
		}
		index.push_back(neatcase[n].size());
		//遍历index，按照前闭后开的形式将tmpV分成几分，
		for (int m = 0; m < index.size() - 1; m++)
		{
			int count = index.at(m + 1) - index.at(m);
			QVector<BOXINFO> tmp = tmpV.mid(index.at(m), count);
			result.push_back(tmp);
		}
	}

	neatcase.clear();
	neatcase = result;
}

bool AlgorithmUnstackCommon::getBlockInfo(QVector<BOXINFO> block, BOXINFO& cases_one)
{
	if (block.size() == 0) {
		return false;
	}

	if (block.size() == 1) {
		cases_one.SkuLength = block[0].SkuLength;
		cases_one.SkuWidth = block[0].SkuWidth;
		cases_one.id = block[0].id;
		cases_one.Position = block[0].Position;
		cases_one.Child = { block[0].id };
		cases_one.cloud = block[0].cloud;
		return true;
	}

	cases_one.SkuLength = qSqrt(qPow(block[0].Position.SkuPosX - block[block.size() - 1].Position.SkuPosX, 2) + qPow(block[0].Position.SkuPosY - block[block.size() - 1].Position.SkuPosY, 2))
		+ block[0].SkuWidth / 2 + block[block.size() - 1].SkuWidth / 2;

	QVector<int> skuX;
	QVector<int> skuY;
	int WidthSum = 0;
	float angle = 0;
	for (int i = 0; i < block.size(); i++) {
		skuX.push_back(block[i].Position.SkuPosX);
		skuY.push_back(block[i].Position.SkuPosY);
		WidthSum += block[i].SkuLength;
		float tempA = block[i].Position.SkuPosZA;
		if (abs(tempA + 90) < UNSTACK_ANGLE_FIVE) {
			angle = angle + tempA + 180;
		} else if (abs(tempA - 180) < UNSTACK_ANGLE_FIVE) {
			angle = angle + (tempA - 180);
		} else if (abs(tempA + 180) < UNSTACK_ANGLE_FIVE) {
			angle = angle + (tempA + 180);
		} else {
			angle += tempA;
		}
	}

	if (relativeX == 1)
		std::sort(skuX.begin(), skuX.end(), compbyintXP);
	else if (relativeX == 0)
		std::sort(skuX.begin(), skuX.end(), compbyintXN);

	if (relativeY == 1)
		std::sort(skuY.begin(), skuY.end(), compbyintYP);
	else if (relativeY == 0)
		std::sort(skuY.begin(), skuY.end(), compbyintYN);

	float skuWidthAvg = WidthSum / block.size();
	angle = angle / block.size() + 90;

	int sku1X = block[0].Position.SkuPosX;
	int sku1Y = block[0].Position.SkuPosY;
	int sku2X = block[block.size() - 1].Position.SkuPosX;
	int sku2Y = block[block.size() - 1].Position.SkuPosY;

	if (angle > 180) {
		angle -= 180;
	} else if (angle < -180) {
		angle += 180;
	}
	cases_one.Position.SkuPosZA = angle;

	if (abs(angle - 90) < UNSTACK_ANGLE_FIVE || abs(angle + 90) < UNSTACK_ANGLE_FIVE) {
		cases_one.SkuWidth = skuWidthAvg + abs(skuX[0] - skuX[skuX.size() - 1]);
		cases_one.Position.SkuPosX = skuX[0];
		cases_one.Position.SkuPosY = (sku1Y + sku2Y) / 2;
		cases_one.Position.SkuPosZ = block[0].Position.SkuPosZ;
	} else if (abs(angle) < UNSTACK_ANGLE_FIVE || abs(angle + 180) < UNSTACK_ANGLE_FIVE || abs(angle - 180) < UNSTACK_ANGLE_FIVE) {
		cases_one.SkuWidth = skuWidthAvg + abs(skuY[0] - skuY[skuY.size() - 1]);
		cases_one.Position.SkuPosY = skuY[0];
		cases_one.Position.SkuPosX = (sku1X + sku2X) / 2;
		cases_one.Position.SkuPosZ = block[0].Position.SkuPosZ;
	} else {
		return false;
	}
	for (int i = 0; i < block.size(); i++) {
		cases_one.Child.push_back(block[i].id);
	}

	PointCloud::Ptr cloudAll(new PointCloud);
	for (int i = 0; i < block.size(); i++) {
		PointCloud::Ptr temp = block.at(i).cloud;
		*cloudAll += *temp;
	}
	cases_one.cloud = cloudAll;

	return true;
}
int AlgorithmUnstackCommon::trySeveralGrab(QVector<QVector<BOXINFO>>& neatcase, PointCloud::Ptr cloud_world_ptr,
	const int max, const JigSetting& jig, GrabInfo& grab, BOXINFO& target)
{
	std::vector<int> vindex;

	float deltax = 0.0;
	float deltay = 0.0;

	//箱体之间X与Y的差值是否符合多抓要求
	for (size_t n = 0; n < neatcase.size(); n++) {
		for (size_t m = 0; m < neatcase[n].size() - 1; m++) {
			deltax = abs(neatcase[n][m].Position.SkuPosX - neatcase[n][m + 1].Position.SkuPosX);
			deltay = abs(neatcase[n][m].Position.SkuPosY - neatcase[n][m + 1].Position.SkuPosY);
			if (abs(neatcase[n][m].Position.SkuPosZA - 90) < UNSTACK_ANGLE_FIVE || abs(neatcase[n][m].Position.SkuPosZA + 90) < UNSTACK_ANGLE_FIVE) {
				if (deltax >= (neatcase[n][m].SkuWidth + 50) || deltay > 40) {
					vindex.push_back(n);
					break;
				}
			}

			else if (abs(neatcase[n][m].Position.SkuPosZA) < UNSTACK_ANGLE_FIVE) {
				if (deltay > (neatcase[n][m].SkuWidth + 50) || deltax > 40) {
					vindex.push_back(n);
					break;
				}
			}

		}
	}

	//把距离过远的删除  一删删一行或一列
	for (size_t q = 0; q < vindex.size(); q++) {
		neatcase.remove(vindex[q]);
		for (size_t p = 0; p < vindex.size(); p++) {
			vindex[p] -= 1;
		}
	}


	if (neatcase.size() == 0)
		return 0;

	for (int i = 0; i < neatcase.size(); i++) {
		QVector<BOXINFO> temp;
		int casenum = max;
		if (casenum == 0) {
			return 0;
		} else {
			if (neatcase[i].size() == 1) {
				continue;
			} else if (neatcase[i].size() <= casenum) {
				temp = neatcase[i];
			} else {
				temp = QVector<BOXINFO>(neatcase[i].begin(), neatcase[i].begin() + casenum);
			}

			PointCloud::Ptr casepoint_cases;
			casepoint_cases = GetCloudWithCaseVec(temp, cloud_world_ptr);//按照箱子位置构建的点云规则的箱子点云    可一次抓取的几个箱子点云
			casepoint_cases->height = 1;
			casepoint_cases->width = casepoint_cases->points.size();

			if (bSave) {
				pcl::io::savePCDFile("cloud_withcase.pcd", *casepoint_cases);

			}

			PointCloud::Ptr point_check;
			point_check = GetCloudWithoutCaseVec(temp, cloud_world_ptr);//去除箱子区域的点云

			if (bSave) {
				pcl::io::savePCDFile("cloud_doublewithoutcase.pcd", *point_check);

			}

			if (!getBlockInfo(temp, target))
				return 0;
			grab = GetGrabPosition(target, jig, point_check);
			if (grab.id > 0) {
				QString text = {};
				for (auto& iter : target.Child) {
					text += QString::number(iter) + ",";
				}
				log_trace("target : {}", text);
				//扣掉箱子处的点云
				return temp.size();
			}

		}
	}
	return 0;
}

float AlgorithmUnstackCommon::GetAngleTrans(float angle, float anglexX, float angleyY)
{
	float angle2 = angle;
	//先判断angle在哪个数附近

	QVector<float> delt;
	float delt1 = abs(angle + 180);
	delt.append(delt1);
	float delt2 = abs(angle + 90);
	delt.append(delt2);
	float delt3 = abs(angle);
	delt.append(delt3);
	float delt4 = abs(angle - 90);
	delt.append(delt4);
	float delt5 = abs(angle - 180);
	delt.append(delt5);

	auto min = std::min_element(std::begin(delt), std::end(delt));

	int positionmin = std::distance(std::begin(delt), min);
	qDebug() << "biggest = " << positionmin;

	if (positionmin == 0) {
		angle2 = -180;
	} else if (positionmin == 1) {
		angle2 = -90;
	} else if (positionmin == 2) {
		angle2 = 0;
	} else if (positionmin == 3) {
		angle2 = 90;
	} else if (positionmin == 4) {
		angle2 = 180;
	}

	angle2 = angle2 + anglexX;
	angle = angle + anglexX;
	while (true) {
		if (angle2 == anglexX || angle2 == angleyY) {
			if (angle > 180) {
				angle -= 360;
				return angle;
			} else if (angle < -180) {
				angle += 360;
				return angle;
			} else {
				return angle;
			}
		} else {
			angle = angle + 180;
			if (angle >= 180) {
				angle -= 360;
			} else if (angle < -180) {
				angle += 360;
			}

			angle2 = angle2 + 180;
			if (angle2 >= 180) {
				angle2 -= 360;
			} else if (angle2 < -180) {
				angle2 += 360;
			}
		}
	}
}

void AlgorithmUnstackCommon::parrProcess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr pass_cloud)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filteredx(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filteredy(new pcl::PointCloud<pcl::PointXYZ>());

	pcl::PassThrough<pcl::PointXYZ>pass;
	//沿x轴过滤
	pass.setInputCloud(cloud);    //输入点云
	pass.setFilterFieldName("x");
	pass.setFilterLimits(xlimit_min / 1000, xlimit_max / 1000);    //选取0-1之间
	pass.filter(*cloud_filteredx);    //过滤

	//沿y轴过滤
	pass.setInputCloud(cloud_filteredx);    //输入点云
	pass.setFilterFieldName("y");
	pass.setFilterLimits(ylimit_min / 1000, ylimit_max / 1000);    //选取0-1之间
	//pass.setFilterLimitsNegative(true);    //可选择0-1之间数据保留还是舍弃
	pass.filter(*cloud_filteredy);    //过滤

	//沿z轴过滤
	pass.setInputCloud(cloud_filteredy);    //输入点云
	pass.setFilterFieldName("z");
	pass.setFilterLimits(zlimit_min / 1000, zlimit_max / 1000);    //选取0-1之间
	//pass.setFilterLimitsNegative(true);    //可选择0-1之间数据保留还是舍弃
	pass.filter(*pass_cloud);    //过滤
}

//给定点云，将点云绘制到rgb上，用于绘制托盘和夹具
void AlgorithmUnstackCommon::DrawPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Scalar sc, cv::Mat& image)
{
	//1.转到相机坐标系
	cv::Mat externalMatrixToCam = externalMatrix.inv();
	Eigen::Matrix4f toCam = vision3d.Mat4d2Matrix4f(externalMatrixToCam);
	double dx = toCam(0, 3) / 1000;
	double dy = toCam(1, 3) / 1000;
	double dz = toCam(2, 3) / 1000;
	toCam(0, 3) = dx;
	toCam(1, 3) = dy;
	toCam(2, 3) = dz;
	pcl::PointCloud<pcl::PointXYZ>::Ptr palltCamCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*cloud, *palltCamCloud, toCam);

	//生成四个角点坐标
	pcl::PointXYZ left_top_point(palltCamCloud->points.at(0).x, palltCamCloud->points.at(0).y, palltCamCloud->points.at(0).z);
	pcl::PointXYZ left_bottom_point(palltCamCloud->points.at(1).x, palltCamCloud->points.at(1).y, palltCamCloud->points.at(1).z);
	pcl::PointXYZ right_bottom_point(palltCamCloud->points.at(2).x, palltCamCloud->points.at(2).y, palltCamCloud->points.at(2).z);
	pcl::PointXYZ right_top_point(palltCamCloud->points.at(3).x, palltCamCloud->points.at(3).y, palltCamCloud->points.at(3).z);

	//4个角点的顺序不对，需要排序后，再计算底部中心点
	//基于相机方向（向下为Y增加，向右为X增加）且空间中箱子基本不倾斜，区分4个角点
	std::vector<pcl::PointXYZ> vPoint = { left_top_point ,left_bottom_point ,right_bottom_point ,right_top_point };
	std::vector<float> vY = { left_top_point.y ,left_bottom_point.y ,right_bottom_point.y ,right_top_point.y };
	std::vector<int> index_by_Y = sort_indexes(vY, 1);//获取较大的Y值索引  0 1 是Y较大的下面的点  2 3是Y较小的上面的点 
	if (vPoint[index_by_Y[0]].x < vPoint[index_by_Y[1]].x) {
		left_bottom_point = vPoint[index_by_Y[0]];
		right_bottom_point = vPoint[index_by_Y[1]];
	} else {
		left_bottom_point = vPoint[index_by_Y[1]];
		right_bottom_point = vPoint[index_by_Y[0]];
	}
	if (vPoint[index_by_Y[2]].x < vPoint[index_by_Y[3]].x) {
		left_top_point = vPoint[index_by_Y[2]];
		right_top_point = vPoint[index_by_Y[3]];
	} else {
		left_top_point = vPoint[index_by_Y[3]];
		right_top_point = vPoint[index_by_Y[2]];
	}

	if (1) {
		//绘制外接矩形
		std::vector<pcl::PointXYZ> vPoint = { left_top_point ,left_bottom_point ,right_bottom_point ,right_top_point };
		//3D点云数据转化
		std::vector<cv::Point3f> CloudPoints;//空间点
		for (const auto& point : vPoint) {
			{
				CloudPoints.push_back(cv::Point3f(point.x, point.y, point.z));
			}
		}

		//把点投影到2D平面上
		Eigen::Matrix4d CamPose = vision3d.EulerPose2MatrixPose(0, 0, 0, 0, 0, 0);
		cv::Mat CamPoseMat = vision3d.Matrix4d2Mat4d(CamPose);
		Mat Rvec, Tvec;
		vision.HomMat3dToRVT(CamPoseMat, &Rvec, &Tvec);

		std::vector<cv::Point2f> ImagePoints;//绘制箭头投影图像点
		cv::projectPoints(CloudPoints, Rvec, Tvec, intrinsicMatrix, distortionMatrix, ImagePoints);

		std::vector<cv::Point2f> ImagePoints2{ ImagePoints[1],ImagePoints[2] ,ImagePoints[3],ImagePoints[0] };
		//绘制外接矩形
		vision.lines(image, ImagePoints, ImagePoints2, sc, 3);
	}
}

void AlgorithmUnstackCommon::DrawTray(cv::Mat& image)
{
	cv::Scalar color;
	color[0] = 255; color[1] = 255; color[2] = 0;
	PointCloud::Ptr palltWordCloud(new PointCloud);//此为世界坐标系
	PointT point_temp1;
	point_temp1.x = xlimit_min / 1000;
	point_temp1.y = ylimit_min / 1000;
	point_temp1.z = maxHeight / 1000;
	palltWordCloud->points.push_back(point_temp1);

	PointT point_temp2;
	point_temp2.x = xlimit_min / 1000;
	point_temp2.y = ylimit_max / 1000;
	point_temp2.z = maxHeight / 1000;
	palltWordCloud->points.push_back(point_temp2);

	PointT point_temp3;
	point_temp3.x = xlimit_max / 1000;
	point_temp3.y = ylimit_min / 1000;
	point_temp3.z = maxHeight / 1000;
	palltWordCloud->points.push_back(point_temp3);

	PointT point_temp4;
	point_temp4.x = xlimit_max / 1000;
	point_temp4.y = ylimit_max / 1000;
	point_temp4.z = maxHeight / 1000;
	palltWordCloud->points.push_back(point_temp4);
	DrawPointCloud(palltWordCloud, color, image);
}

//画出识别出来的箱子
void AlgorithmUnstackCommon::DrowBox(cv::Mat& image, cv::RotatedRect rect, const int id)
{
	//通过Rect找到四个角点坐标
	cv::Point2f box[4];
	rect.points(box);

	cv::Point p1(box[0].x, box[0].y);
	cv::Point p2(box[1].x, box[1].y);
	cv::Point p3(box[2].x, box[2].y);
	cv::Point p4(box[3].x, box[3].y);
	std::vector<Point> pts = { p1,p2,p3,p4 };
	cv::polylines(image, pts, -1, (0, 0, 255), 2);

	cv::Point2f temp = {};
	for (auto& iter : box) {
		temp.x += iter.x;
		temp.y += iter.y;
	}
	temp.x /= 4;
	temp.y /= 4;

	cv::putText(image, QString::number(id).toStdString(), temp, 3, 1, cv::Scalar(0, 255, 255), 3, 8);
}

//画出抓取坐标以及数值
void AlgorithmUnstackCommon::DrowCoordinate(SKUPOSITION position, cv::Mat& rgb, float delx, float delty, int count)
{
	//显示的位置
	QString x = QString::number(position.SkuPosX - delx);
	QString y = QString::number(position.SkuPosY - delty);
	QString z = QString::number(position.SkuPosZ);
	QString a = QString::number(position.SkuPosZA);

	QString info = "(" + x + ", " + y + ", " + z + ", " + a + ")";


	if (count == 0) {
		cv::Point2f temp;;
		temp.y = 50;
		temp.x = 50;
		//绘制坐标系文字
		putText(rgb, info.toStdString(), temp, 3, 1, Scalar(0, 255, 0), 3, 8);
	} else {
		cv::Point2f temp;;
		temp.y = 100;
		temp.x = 50;
		//绘制坐标系文字
		putText(rgb, info.toStdString(), temp, 3, 1, Scalar(0, 255, 0), 3, 8);
	}

	//画出坐标系
	Eigen::Matrix4d pose3D = vision3d.EulerPose2MatrixPose(position.SkuPosX - delx, position.SkuPosY - delty, position.SkuPosZ, 0, 0, -position.SkuPosZA);
	cv::Mat pose3DMat = vision3d.Matrix4d2Mat4d(pose3D);
	cv::Mat pose3DCamMat = externalMatrix.inv() * pose3DMat;
	vision.Disp3dCoordSystem(&rgb, pose3DCamMat, intrinsicMatrix, distortionMatrix, 200, 2, "", false);
}

void AlgorithmUnstackCommon::DrawSucker(const Position& grab, const JigSetting& jigInfo, cv::Mat& image)
{
	//变换矩阵
	Eigen::Affine3f transform = Eigen::Affine3f::Identity();
	transform.translation() << (grab.x) / 1000, (grab.y) / 1000, grab.z / 1000;
	transform.rotate(Eigen::AngleAxisf(grab.a * M_PI / 180, Eigen::Vector3f::UnitZ()));
	//颜色
	cv::Scalar color;
	color[0] = 255; color[1] = 255; color[2] = 255;

	//生成吸盘
	PointCloud::Ptr sucker_convex(new PointCloud);
	sucker_convex->push_back(PointT(0.0, 0.0, 0.0));
	sucker_convex->push_back(PointT(0.0, (double)jigInfo.width/ 1000, 0));
	sucker_convex->push_back(PointT((double)jigInfo.length / 1000, (double)jigInfo.width / 1000, 0));
	sucker_convex->push_back(PointT((double)jigInfo.length / 1000 , 0.0, 0));
	//坐标变换
	PointCloud::Ptr cloud_suckerconvex_world(new PointCloud);
	pcl::transformPointCloud(*sucker_convex, *cloud_suckerconvex_world, transform);
	//画出矩形
	DrawPointCloud(cloud_suckerconvex_world, color, image);
	if (jigInfo.type == 1) {
		//生成侧边吸盘
		PointCloud::Ptr sucker_sode(new PointCloud);
		sucker_sode->push_back(PointT(0, 0, 0));
		sucker_sode->push_back(PointT(0, jigInfo.width / 1000, 0));;
		sucker_sode->push_back(PointT(-20, jigInfo.width / 1000, 0));
		sucker_sode->push_back(PointT(-20, 0, 0));
		//坐标变换
		PointCloud::Ptr cloud_suckerSide_world(new PointCloud);
		pcl::transformPointCloud(*sucker_sode, *cloud_suckerSide_world, transform);
		//画出矩形
		DrawPointCloud(cloud_suckerSide_world, color, image);
	}

	Eigen::Matrix4d pose3D = vision3d.EulerPose2MatrixPose(grab.x,grab.y, grab.z, 0, 0, -grab.a);
	cv::Mat pose3DMat = vision3d.Matrix4d2Mat4d(pose3D);
	cv::Mat pose3DCamMat = externalMatrix.inv() * pose3DMat;
	vision.Disp3dCoordSystem(&image, pose3DCamMat, intrinsicMatrix, distortionMatrix, 200, 2, "", false);
}

//画出吸盘坐标，如果是带测吸盘的吸盘，要画出吸盘朝向
void AlgorithmUnstackCommon::DrowSucker(float angle, BOXINFO recog, float deltx, float delty, cv::Mat& image)
{
	//变换矩阵
	Eigen::Affine3f transform = Eigen::Affine3f::Identity();
	transform.translation() << (recog.Position.SkuPosX + deltx) / 1000, (recog.Position.SkuPosY + delty) / 1000, recog.Position.SkuPosZ / 1000;
	transform.rotate(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()));
	std::cout << "3:" << transform.matrix() << std::endl;
	//颜色
	cv::Scalar color;
	color[0] = 255; color[1] = 255; color[2] = 255;

	if (suckerType == 1) {  //纯吸盘
		//生成吸盘
		PointCloud::Ptr sucker_convex(new PointCloud);
		sucker_convex->push_back(PointT(800 / 2 / 1000, 500/ 2 / 1000, 0));
		sucker_convex->push_back(PointT(-800/ 2 / 1000, 500 / 2 / 1000, 0));
		sucker_convex->push_back(PointT(800 / 2 / 1000, -500/ 2 / 1000, 0));
		sucker_convex->push_back(PointT(-800 / 2 / 1000, -500 / 2 / 1000, 0));

		//坐标变换
		PointCloud::Ptr cloud_suckerconvex_world(new PointCloud);
		pcl::transformPointCloud(*sucker_convex, *cloud_suckerconvex_world, transform);
		//画出矩形
		DrawPointCloud(cloud_suckerconvex_world, color, image);
	} else { //带侧边吸盘
		//生成纯吸盘
		PointCloud::Ptr sucker_convex(new PointCloud);
		sucker_convex->push_back(PointT(800.0 / 2 / 1000, 500.0 / 2 / 1000, 0));
		sucker_convex->push_back(PointT(-800.0 / 2 / 1000, 500.0 / 2 / 1000, 0));
		sucker_convex->push_back(PointT(800.0 / 2 / 1000, -500.0 / 2 / 1000, 0));
		sucker_convex->push_back(PointT(-800.0 / 2 / 1000, -500.0 / 2 / 1000, 0));
		//坐标变换
		PointCloud::Ptr cloud_suckerconvex_world(new PointCloud);
		pcl::transformPointCloud(*sucker_convex, *cloud_suckerconvex_world, transform);
		//画出矩形
		DrawPointCloud(cloud_suckerconvex_world, color, image);

		//生成侧边吸盘
		PointCloud::Ptr sucker_sode(new PointCloud);
		sucker_sode->push_back(PointT(suker_size.x() / 2 / 1000, -suker_size.y() / 2 / 1000, 0));
		sucker_sode->push_back(PointT(-suker_size.x() / 2 / 1000, -suker_size.y() / 2 / 1000, 0));;
		sucker_sode->push_back(PointT(suker_size.x() / 2 / 1000, (-suker_size.y() / 2 - 200) / 1000, 0));
		sucker_sode->push_back(PointT(-suker_size.x() / 2 / 1000, (-suker_size.y() / 2 - 200) / 1000, 0));
		//坐标变换
		PointCloud::Ptr cloud_suckerSide_world(new PointCloud);
		pcl::transformPointCloud(*sucker_sode, *cloud_suckerSide_world, transform);
		//画出矩形
		DrawPointCloud(cloud_suckerSide_world, color, image);
	}
}

int AlgorithmUnstackCommon::ParseRecogRecogResult(const QList<BoxRect>& input, QVector<RECGRESULT>& result)
{
	int num = 0;
	for (auto& iter : input) {
		RECGRESULT temp = {};
		temp.id = iter.id;
		temp.P1.setX(iter.x1);
		temp.P2.setX(iter.x2);
		temp.P3.setX(iter.x3);
		temp.P4.setX(iter.x4);
		temp.P1.setY(iter.y1);
		temp.P2.setY(iter.y2);
		temp.P3.setY(iter.y3);
		temp.P4.setY(iter.y4);
		result.push_back(temp);
		num++;
	}
	return num;
}

QString imencodeToBase64(const cv::Mat& Image, const std::string& ext)
{
	if (Image.empty()) {
		return QString("");
	}
	//图片编码
	std::vector<uchar> vuBuf;
	/*cv::imencode(".jpg", Image, vuBuf);*/
	cv::imencode(ext, Image, vuBuf);
	//std::vector<uchar>转QByteArray
	QByteArray qbaImage(reinterpret_cast<const char*>(vuBuf.data()), vuBuf.size());//https://www.jb51.cc/c/116955.html
	QByteArray qbaImage1 = qbaImage.toBase64();

	QTextCodec* tc = QTextCodec::codecForName("UTF8");//https://blog.csdn.net/liukang325/article/details/80986472?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-1.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromMachineLearnPai2%7Edefault-1.control
	QString qbaImage2 = tc->toUnicode(qbaImage1);

	return qbaImage2;
}

QVector<float> Eul2Quat(float yaw, float roll, float pitch)
{
	QVector<float> result;

	Eigen::Vector3d ea(yaw * M_PI / 180.0, roll * M_PI / 180.0, pitch * M_PI / 180.0);
	Eigen::Matrix3d R;
	R = Eigen::AngleAxisd(ea[0], ::Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], ::Eigen::Vector3d::UnitY())
		* Eigen::AngleAxisd(ea[2], ::Eigen::Vector3d::UnitX());
	Eigen::Quaterniond q;
	q = R;

	if (abs(q.w()) < 0.0001) {
		result.push_back(0);
	} else {
		result.push_back(q.w());
	}

	if (abs(q.x()) < 0.0001) {
		result.push_back(0);
	} else {
		result.push_back(q.x());
	}
	if (abs(q.y()) < 0.0001) {
		result.push_back(0);
	} else {
		result.push_back(q.y());
	}
	if (abs(q.z()) < 0.0001) {
		result.push_back(0);
	} else {
		result.push_back(q.z());
	}

	return result;
}

bool AlgorithmUnstackCommon::SortSkuPos(QVector<BoxInfo>& pos)
{
	// x由小变大，y由小变大
	auto fun1 = [](const BoxInfo& p1, const BoxInfo& p2) {
		int num2 = p2.pos.x + p2.pos.y;
		int num1 = p1.pos.x + p1.pos.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.pos.y < p2.pos.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	// x由大变小，y由小变大
	auto fun2 = [](const BoxInfo& p1, const BoxInfo& p2) {
		int num2 = -p2.pos.x + p2.pos.y;
		int num1 = -p1.pos.x + p1.pos.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.pos.y < p2.pos.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	// x 由小变大， y由大变小
	auto fun3 = [](const BoxInfo& p1, const BoxInfo& p2) {
		int num2 = p2.pos.x - p2.pos.y;
		int num1 = p1.pos.x - p1.pos.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.pos.y > p2.pos.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	// x由大变小，y由大变小
	auto fun4 = [](const BoxInfo& p1, const BoxInfo& p2) {
		int num2 = -p2.pos.x - p2.pos.y;
		int num1 = -p1.pos.x - p1.pos.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.pos.y > p2.pos.y) {
				return true;
			} else {
				return false;
			}
		}
	};

	switch (m_dir) {
	case 0:
		std::sort(pos.begin(), pos.end(), fun4);
		break;
	case 1:
		std::sort(pos.begin(), pos.end(), fun3);
		break;
	case 2:
		std::sort(pos.begin(), pos.end(), fun1);
		break;
	case 3:
		std::sort(pos.begin(), pos.end(), fun2);
		break;
	default:
		return false;
		break;
	}

	return true;
}

bool AlgorithmUnstackCommon::SortGrabPos(QVector<GrabInfo>& pos)
{
	// x由小变大，y由小变大
	auto fun1 = [](const GrabInfo& p1, const GrabInfo& p2) {
		int num2 = p2.jigInsku.x + p2.jigInsku.y;
		int num1 = p1.jigInsku.x + p1.jigInsku.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.jigInsku.y < p2.jigInsku.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	// x由大变小，y由小变大
	auto fun2 = [](const GrabInfo& p1, const GrabInfo& p2) {
		int num2 = -p2.jigInsku.x + p2.jigInsku.y;
		int num1 = -p1.jigInsku.x + p1.jigInsku.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.jigInsku.y < p2.jigInsku.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	// x 由小变大， y由大变小
	auto fun3 = [](const GrabInfo& p1, const GrabInfo& p2) {
		int num2 = p2.jigInsku.x - p2.jigInsku.y;
		int num1 = p1.jigInsku.x - p1.jigInsku.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.jigInsku.y > p2.jigInsku.y) {
				return true;
			} else {
				return false;
			}
		}
	};
	// x由大变小，y由大变小
	auto fun4 = [](const GrabInfo& p1, const GrabInfo& p2) {
		int num2 = -p2.jigInsku.x - p2.jigInsku.y;
		int num1 = -p1.jigInsku.x - p1.jigInsku.y;
		if (num2 > num1) {
			return true;
		} else if (num2 < num1) {
			return false;
		} else {
			if (p1.jigInsku.y > p2.jigInsku.y) {
				return true;
			} else {
				return false;
			}
		}
	};

	switch (m_dir) {
	case 0:
		std::sort(pos.begin(), pos.end(), fun4);
		break;
	case 1:
		std::sort(pos.begin(), pos.end(), fun3);
		break;
	case 2:
		std::sort(pos.begin(), pos.end(), fun1);
		break;
	case 3:
		std::sort(pos.begin(), pos.end(), fun2);
		break;
	default:
		return false;
		break;
	}

	return true;
}
